From 1e529a96b9dcfdb95fe785e55eb67834234630f0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 5 Mar 2017 18:00:46 +0000 Subject: [PATCH] PG CLI additions 5 --- src/main/cms/cms_menu_vtx.c | 35 +++-- src/main/config/parameter_group_ids.h | 3 +- src/main/fc/cli.c | 83 +++++++++++- src/main/fc/config.c | 4 +- src/main/fc/fc_msp.c | 2 +- src/main/io/vtx.c | 16 ++- src/main/io/vtx.h | 2 + src/main/target/COLIBRI_RACE/i2c_bst.c | 180 ++++++++++++------------- 8 files changed, 212 insertions(+), 113 deletions(-) diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 238dac0008..91bde81258 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -38,8 +38,19 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "fc/config.h" + +#include "io/vtx.h" + static bool featureRead = false; -static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; +static uint8_t cmsx_featureVtx = 0; +static uint8_t cmsx_vtxBand; +static uint8_t cmsx_vtxChannel; +#ifdef VTX +static uint8_t cmsx_vtxMode; +static uint16_t cmsx_vtxMhz; +#endif +static bool cmsx_vtxPower; static long cmsx_Vtx_FeatureRead(void) { @@ -73,13 +84,20 @@ static const char * const vtxBandNames[] = { static OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]}; static OSD_UINT8_t entryVtxChannel = {&cmsx_vtxChannel, 1, 8, 1}; +#ifdef VTX +static OSD_UINT8_t entryVtxMode = {&cmsx_vtxMode, 0, 2, 1}; +static OSD_UINT16_t entryVtxMhz = {&cmsx_vtxMhz, 5600, 5950, 1}; +#endif // VTX static void cmsx_Vtx_ConfigRead(void) { #ifdef VTX cmsx_vtxBand = vtxConfig()->vtx_band; cmsx_vtxChannel = vtxConfig()->vtx_channel + 1; + cmsx_vtxMode = vtxConfig()->vtx_mode; + cmsx_vtxMhz = vtxConfig()->vtx_mhz; #endif // VTX + cmsx_vtxPower = vtxConfig()->vtx_power; #ifdef USE_RTC6705 cmsx_vtxBand = vtxConfig()->vtx_channel / 8; @@ -90,12 +108,15 @@ static void cmsx_Vtx_ConfigRead(void) static void cmsx_Vtx_ConfigWriteback(void) { #ifdef VTX - vtxConfig()->vtx_band = cmsx_vtxBand; - vtxConfig()->vtx_channel = cmsx_vtxChannel - 1; + vtxConfigMutable()->vtx_band = cmsx_vtxBand; + vtxConfigMutable()->vtx_channel = cmsx_vtxChannel - 1; + vtxConfigMutable()->vtx_mode = cmsx_vtxMode; + vtxConfigMutable()->vtx_mhz = cmsx_vtxMhz; #endif // VTX + vtxConfigMutable()->vtx_power = cmsx_vtxPower; #ifdef USE_RTC6705 - vtxConfig()->vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; + vtxConfigMutable()->vtx_channel = cmsx_vtxBand * 8 + cmsx_vtxChannel - 1; #endif // USE_RTC6705 } @@ -116,10 +137,6 @@ static long cmsx_Vtx_onExit(const OSD_Entry *self) return 0; } -#ifdef VTX -static OSD_UINT8_t entryVtxMode = {&vtxConfig()->vtx_mode, 0, 2, 1}; -static OSD_UINT16_t entryVtxMhz = {&vtxConfig()->vtx_mhz, 5600, 5950, 1}; -#endif // VTX static OSD_Entry cmsx_menuVtxEntries[] = { @@ -132,7 +149,7 @@ static OSD_Entry cmsx_menuVtxEntries[] = {"BAND", OME_TAB, NULL, &entryVtxBand, 0}, {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0}, #ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &vtxConfig()->vtx_power, 0}, + {"LOW POWER", OME_Bool, NULL, &cmsx_vtxPower, 0}, #endif // USE_RTC6705 {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index ea2e99484b..239e9302e2 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -96,7 +96,8 @@ #define PG_DISPLAY_PORT_MSP_CONFIG 512 #define PG_DISPLAY_PORT_MAX7456_CONFIG 513 #define PG_VCD_CONFIG 514 -#define PG_BETAFLIGHT_END 514 +#define PG_VTX_CONFIG 515 +#define PG_BETAFLIGHT_END 515 // OSD configuration (subject to change) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 60a81aedc4..63d9498e71 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -505,6 +505,14 @@ static const clivalue_t valueTable[] = { { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 128 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, +#if defined(GYRO_USES_SPI) +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) + { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) }, +#endif +#if defined(USE_MPU_DATA_READY_SIGNAL) + { "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_isr_update) }, +#endif +#endif // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, @@ -539,9 +547,10 @@ static const clivalue_t valueTable[] = { { "rssi_channel", VAR_INT8 | MASTER_VALUE, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_channel) }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_scale) }, { "rssi_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_invert) }, - { "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) }, - { "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationInterval) }, - { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, + { "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) }, + { "rc_interp_ch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationChannels) }, + { "rc_interp_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationInterval) }, + { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 13 }, PG_RX_CONFIG, offsetof(rxConfig_t, max_aux_channel) }, #ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) }, @@ -551,9 +560,17 @@ static const clivalue_t valueTable[] = { { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif - { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = {1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, + { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = {1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_max_usec) }, +#ifdef STM32F4 + { "serialrx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) }, +#endif + +// PG_PWM_CONFIG +#if defined(USE_PWM) + { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PWM_CONFIG, offsetof(pwmConfig_t, inputFilteringMode) }, +#endif // PG_BLACKBOX_CONFIG #ifdef BLACKBOX @@ -575,6 +592,10 @@ static const clivalue_t valueTable[] = { { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) }, { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) }, +// PG_THROTTLE_CORRECTION_CONFIG + { "thr_corr_value", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 150 }, PG_THROTTLE_CORRECTION_CONFIG, offsetof(throttleCorrectionConfig_t, throttle_correction_value) }, + { "thr_corr_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 900 }, PG_THROTTLE_CORRECTION_CONFIG, offsetof(throttleCorrectionConfig_t, throttle_correction_angle) }, + // PG_FAILSAFE_CONFIG { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_delay) }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_off_delay) }, @@ -610,6 +631,12 @@ static const clivalue_t valueTable[] = { { "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useConsumptionAlerts) }, { "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) }, +// PG_BEEPER_DEV_CONFIG +#ifdef BEEPER + { "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, isInverted) }, + { "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, isOpenDrain) }, +#endif + // PG_MIXER_CONFIG { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motor_direction) }, @@ -625,6 +652,7 @@ static const clivalue_t valueTable[] = { { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 498 }, PG_SERVO_CONFIG, offsetof(servoConfig_t, dev.servoPwmRate) }, { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400}, PG_SERVO_CONFIG, offsetof(servoConfig_t, servo_lowpass_freq) }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SERVO_CONFIG, offsetof(servoConfig_t, tri_unarmed_servo) }, + { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT }, PG_SERVO_CONFIG, offsetof(servoConfig_t, channelForwardingStartChannel) }, #endif // PG_CONTROLRATE_PROFILES @@ -666,6 +694,18 @@ static const clivalue_t valueTable[] = { { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, #endif +// PG_NAVIGATION_CONFIG +#ifdef GPS + { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, gps_wp_radius) }, + { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_controls_heading) }, + { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_min) }, + { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_max) }, + { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) }, +#endif + +// PG_AIRPLANE_CONFIG + { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_dir) }, + // PG_RC_CONTROLS_CONFIG { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) }, { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_fast_change) }, @@ -690,6 +730,7 @@ static const clivalue_t valueTable[] = { { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) }, + { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0.1, 1.0 }, PG_PID_CONFIG, offsetof(pidProfile_t, pidSumLimit) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PITCH]) }, @@ -746,6 +787,13 @@ static const clivalue_t valueTable[] = { #endif // PG_LED_STRIP_CONFIG +#ifdef LED_STRIP + { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ledstrip_visual_beeper) }, +#endif + +#ifdef USE_SDCARD + { "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, useDma) }, +#endif // PG_OSD_CONFIG #ifdef OSD @@ -784,6 +832,30 @@ static const clivalue_t valueTable[] = { { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, #endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, +#ifdef VTX + { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_band) }, + { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_channel) }, + { "vtx_mode", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_mode) }, + { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 5600, 5950 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_mhz) }, +#endif + +#if defined(USE_RTC6705) + { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 39 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_channel) }, + { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_VTX_CONFIG, offsetof(vtxConfig_t, vtx_power) }, +#endif +#ifdef USE_MAX7456 + { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) }, + { "vcd_h_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -32, 31 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, h_offset) }, + { "vcd_v_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -15, 16 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, v_offset) }, +#endif +#ifdef USE_MSP_DISPLAYPORT + { "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, + { "displayport_msp_row_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, +#endif +#ifdef USE_MAX7456 + { "displayport_max7456_col_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MAX7456_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, + { "displayport_max7456_row_adjust", VAR_INT8| MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MAX7456_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, +#endif }; #else @@ -957,7 +1029,7 @@ static const clivalue_t valueTable[] = { { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servo_lowpass_freq, .config.minmax = { 0, 400} }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->dev.servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, - { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &servoConfig()->channelForwardingStartChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, + { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &servoConfig()->channelForwardingStartChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, #endif { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, @@ -1710,7 +1782,6 @@ typedef union { float float_value; } int_float_value_t; - static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { void *ptr = getValuePointer(var); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 825715c948..3b9a730aa4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -133,8 +133,8 @@ PG_REGISTER_WITH_RESET_FN(ppmConfig_t, ppmConfig, PG_PPM_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(serialPinConfig_t, serialPinConfig, PG_SERIAL_PIN_CONFIG, 0); -PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); #ifdef USE_FLASHFS +PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); #ifdef M25P16_CS_PIN #define FLASH_CONFIG_CSTAG IO_TAG(M25P16_CS_PIN) #else @@ -1106,12 +1106,14 @@ void createDefaultConfig(master_t *config) config->customMotorMixer[i].throttle = 0.0f; } +#ifndef USE_PARAMETER_GROUPS #ifdef VTX config->vtxConfig.vtx_band = 4; //Fatshark/Airwaves config->vtxConfig.vtx_channel = 1; //CH1 config->vtxConfig.vtx_mode = 0; //CH+BAND mode config->vtxConfig.vtx_mhz = 5740; //F0 #endif +#endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a88e3f27b6..dc13cf9159 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1663,7 +1663,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) uint16_t tmp = sbufReadU16(src); #if defined(USE_RTC6705) if (tmp < 40) - vtxConfig()->vtx_channel = tmp; + vtxConfigMutable()->vtx_channel = tmp; if (current_vtx_channel != vtxConfig()->vtx_channel) { current_vtx_channel = vtxConfig()->vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index a61d4533cb..9501cea1ee 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -21,11 +21,6 @@ #ifdef VTX -// Own interfaces -#include "io/vtx.h" -#include "io/osd.h" - -//External dependencies #include "common/maths.h" #include "config/config_eeprom.h" @@ -37,8 +32,19 @@ #include "fc/runtime_config.h" #include "io/beeper.h" +#include "io/osd.h" +#include "io/vtx.h" +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 0); + +PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, + .vtx_band = 4, //Fatshark/Airwaves + .vtx_channel = 1, //CH1 + .vtx_mode = 0, //CH+BAND mode + .vtx_mhz = 5740 //F0 +); + static uint8_t locked = 0; void vtxInit(void) diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 2af923ae99..335d5d6974 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -41,6 +41,8 @@ typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; } vtxConfig_t; +PG_DECLARE(vtxConfig_t, vtxConfig); + void vtxInit(void); void vtxIncrementBand(void); void vtxDecrementBand(void); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 3de3eec9e4..bc4695c2c0 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -622,14 +622,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_SERVO_CONFIGURATIONS: for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - bstWrite16(servoProfile()->servoConf[i].min); - bstWrite16(servoProfile()->servoConf[i].max); - bstWrite16(servoProfile()->servoConf[i].middle); - bstWrite8(servoProfile()->servoConf[i].rate); - bstWrite8(servoProfile()->servoConf[i].angleAtMin); - bstWrite8(servoProfile()->servoConf[i].angleAtMax); - bstWrite8(servoProfile()->servoConf[i].forwardFromChannel); - bstWrite32(servoProfile()->servoConf[i].reversedSources); + bstWrite16(servoParams(i)->min); + bstWrite16(servoParams(i)->max); + bstWrite16(servoParams(i)->middle); + bstWrite8(servoParams(i)->rate); + bstWrite8(servoParams(i)->angleAtMin); + bstWrite8(servoParams(i)->angleAtMax); + bstWrite8(servoParams(i)->forwardFromChannel); + bstWrite32(servoParams(i)->reversedSources); } break; case BST_SERVO_MIX_RULES: @@ -716,7 +716,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_MODE_RANGES: for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + const modeActivationCondition_t *mac = modeActivationConditions(i); const box_t *box = &boxes[mac->modeId]; bstWrite8(box->permanentId); bstWrite8(mac->auxChannelIndex); @@ -726,7 +726,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_ADJUSTMENT_RANGES: for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + const adjustmentRange_t *adjRange = adjustmentRanges(i); bstWrite8(adjRange->adjustmentIndex); bstWrite8(adjRange->auxChannelIndex); bstWrite8(adjRange->range.startStep); @@ -935,7 +935,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #ifdef LED_STRIP case BST_LED_COLORS: for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + hsvColor_t *color = &ledStripConfigMutable()->colors[i]; bstWrite16(color->h); bstWrite8(color->s); bstWrite8(color->v); @@ -944,7 +944,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) case BST_LED_STRIP_CONFIG: for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; bstWrite32(*ledConfig); } break; @@ -1024,12 +1024,12 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } } case BST_SET_ACC_TRIM: - accelerometerConfig()->accelerometerTrims.values.pitch = bstRead16(); - accelerometerConfig()->accelerometerTrims.values.roll = bstRead16(); + accelerometerConfigMutable()->accelerometerTrims.values.pitch = bstRead16(); + accelerometerConfigMutable()->accelerometerTrims.values.roll = bstRead16(); break; case BST_SET_ARMING_CONFIG: - armingConfig()->auto_disarm_delay = bstRead8(); - armingConfig()->disarm_kill_switch = bstRead8(); + armingConfigMutable()->auto_disarm_delay = bstRead8(); + armingConfigMutable()->disarm_kill_switch = bstRead8(); break; case BST_SET_LOOP_TIME: //masterConfig.looptime = bstRead16(); @@ -1047,7 +1047,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) case BST_SET_MODE_RANGE: i = bstRead8(); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); i = bstRead8(); const box_t *box = findBoxByPermenantId(i); if (box) { @@ -1056,7 +1056,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, currentPidProfile); + useRcControlsConfig(modeActivationConditions(0), currentPidProfile); } else { ret = BST_FAILED; } @@ -1067,7 +1067,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) case BST_SET_ADJUSTMENT_RANGE: i = bstRead8(); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + adjustmentRange_t *adjRange = adjustmentRangesMutable(i); i = bstRead8(); if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { adjRange->adjustmentIndex = i; @@ -1109,33 +1109,33 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) case BST_SET_MISC: tmp = bstRead16(); if (tmp < 1600 && tmp > 1400) - rxConfig()->midrc = tmp; + rxConfigMutable()->midrc = tmp; - motorConfig()->minthrottle = bstRead16(); - motorConfig()->maxthrottle = bstRead16(); - motorConfig()->mincommand = bstRead16(); + motorConfigMutable()->minthrottle = bstRead16(); + motorConfigMutable()->maxthrottle = bstRead16(); + motorConfigMutable()->mincommand = bstRead16(); - failsafeConfig()->failsafe_throttle = bstRead16(); + failsafeConfigMutable()->failsafe_throttle = bstRead16(); #ifdef GPS - gpsConfig()->provider = bstRead8(); // gps_type + gpsConfigMutable()->provider = bstRead8(); // gps_type bstRead8(); // gps_baudrate - gpsConfig()->sbasMode = bstRead8(); // gps_ubx_sbas + gpsConfigMutable()->sbasMode = bstRead8(); // gps_ubx_sbas #else bstRead8(); // gps_type bstRead8(); // gps_baudrate bstRead8(); // gps_ubx_sbas #endif - batteryConfig()->multiwiiCurrentMeterOutput = bstRead8(); - rxConfig()->rssi_channel = bstRead8(); + batteryConfigMutable()->multiwiiCurrentMeterOutput = bstRead8(); + rxConfigMutable()->rssi_channel = bstRead8(); bstRead8(); - compassConfig()->mag_declination = bstRead16() * 10; + compassConfigMutable()->mag_declination = bstRead16() * 10; - 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 + batteryConfigMutable()->vbatscale = bstRead8(); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->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 @@ -1151,14 +1151,14 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SUPPORTED_SERVOS) { ret = BST_FAILED; } else { - servoProfile()->servoConf[i].min = bstRead16(); - servoProfile()->servoConf[i].max = bstRead16(); - servoProfile()->servoConf[i].middle = bstRead16(); - servoProfile()->servoConf[i].rate = bstRead8(); - servoProfile()->servoConf[i].angleAtMin = bstRead8(); - servoProfile()->servoConf[i].angleAtMax = bstRead8(); - servoProfile()->servoConf[i].forwardFromChannel = bstRead8(); - servoProfile()->servoConf[i].reversedSources = bstRead32(); + servoParamsMutable(i)->min = bstRead16(); + servoParamsMutable(i)->max = bstRead16(); + servoParamsMutable(i)->middle = bstRead16(); + servoParamsMutable(i)->rate = bstRead8(); + servoParamsMutable(i)->angleAtMin = bstRead8(); + servoParamsMutable(i)->angleAtMax = bstRead8(); + servoParamsMutable(i)->forwardFromChannel = bstRead8(); + servoParamsMutable(i)->reversedSources = bstRead32(); } #endif break; @@ -1168,13 +1168,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SERVO_RULES) { ret = BST_FAILED; } else { - customServoMixers(i)->targetChannel = bstRead8(); - customServoMixers(i)->inputSource = bstRead8(); - customServoMixers(i)->rate = bstRead8(); - customServoMixers(i)->speed = bstRead8(); - customServoMixers(i)->min = bstRead8(); - customServoMixers(i)->max = bstRead8(); - customServoMixers(i)->box = bstRead8(); + customServoMixersMutable(i)->targetChannel = bstRead8(); + customServoMixersMutable(i)->inputSource = bstRead8(); + customServoMixersMutable(i)->rate = bstRead8(); + customServoMixersMutable(i)->speed = bstRead8(); + customServoMixersMutable(i)->min = bstRead8(); + customServoMixersMutable(i)->max = bstRead8(); + customServoMixersMutable(i)->box = bstRead8(); loadCustomServoMixer(); } #endif @@ -1252,51 +1252,51 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) featureSet(bstRead32()); // features bitmap #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { - serialConfig()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } else { - serialConfig()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE; + serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE; } #endif break; case BST_SET_BOARD_ALIGNMENT: - boardAlignment()->rollDegrees = bstRead16(); - boardAlignment()->pitchDegrees = bstRead16(); - boardAlignment()->yawDegrees = bstRead16(); + boardAlignmentMutable()->rollDegrees = bstRead16(); + boardAlignmentMutable()->pitchDegrees = bstRead16(); + boardAlignmentMutable()->yawDegrees = bstRead16(); break; case BST_SET_VOLTAGE_METER_CONFIG: - 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 + batteryConfigMutable()->vbatscale = bstRead8(); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert break; case BST_SET_CURRENT_METER_CONFIG: - batteryConfig()->currentMeterScale = bstRead16(); - batteryConfig()->currentMeterOffset = bstRead16(); - batteryConfig()->currentMeterType = bstRead8(); - batteryConfig()->batteryCapacity = bstRead16(); + batteryConfigMutable()->currentMeterScale = bstRead16(); + batteryConfigMutable()->currentMeterOffset = bstRead16(); + batteryConfigMutable()->currentMeterType = bstRead8(); + batteryConfigMutable()->batteryCapacity = bstRead16(); break; #ifndef USE_QUAD_MIXER_ONLY case BST_SET_MIXER: - mixerConfig()->mixerMode = bstRead8(); + mixerConfigMutable()->mixerMode = bstRead8(); break; #endif case BST_SET_RX_CONFIG: - rxConfig()->serialrx_provider = bstRead8(); - rxConfig()->maxcheck = bstRead16(); - rxConfig()->midrc = bstRead16(); - rxConfig()->mincheck = bstRead16(); - rxConfig()->spektrum_sat_bind = bstRead8(); + rxConfigMutable()->serialrx_provider = bstRead8(); + rxConfigMutable()->maxcheck = bstRead16(); + rxConfigMutable()->midrc = bstRead16(); + rxConfigMutable()->mincheck = bstRead16(); + rxConfigMutable()->spektrum_sat_bind = bstRead8(); if (bstReadDataSize() > 8) { - rxConfig()->rx_min_usec = bstRead16(); - rxConfig()->rx_max_usec = bstRead16(); + rxConfigMutable()->rx_min_usec = bstRead16(); + rxConfigMutable()->rx_max_usec = bstRead16(); } break; case BST_SET_FAILSAFE_CONFIG: - failsafeConfig()->failsafe_delay = bstRead8(); - failsafeConfig()->failsafe_off_delay = bstRead8(); - failsafeConfig()->failsafe_throttle = bstRead16(); + failsafeConfigMutable()->failsafe_delay = bstRead8(); + failsafeConfigMutable()->failsafe_off_delay = bstRead8(); + failsafeConfigMutable()->failsafe_throttle = bstRead16(); break; case BST_SET_RXFAIL_CONFIG: { @@ -1305,18 +1305,18 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; } else { for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) { - rxConfig()->failsafe_channel_configurations[i].mode = bstRead8(); - rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(bstRead16()); + rxConfigMutable()->failsafe_channel_configurations[i].mode = bstRead8(); + rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(bstRead16()); } } } break; case BST_SET_RSSI_CONFIG: - rxConfig()->rssi_channel = bstRead8(); + rxConfigMutable()->rssi_channel = bstRead8(); break; case BST_SET_RX_MAP: for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - rxConfig()->rcmap[i] = bstRead8(); + rxConfigMutable()->rcmap[i] = bstRead8(); } break; case BST_SET_BF_CONFIG: @@ -1324,20 +1324,20 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) #ifdef USE_QUAD_MIXER_ONLY bstRead8(); // mixerMode ignored #else - mixerConfig()->mixerMode = bstRead8(); // mixerMode + mixerConfigMutable()->mixerMode = bstRead8(); // mixerMode #endif featureClearAll(); featureSet(bstRead32()); // features bitmap - rxConfig()->serialrx_provider = bstRead8(); // serialrx_type + rxConfigMutable()->serialrx_provider = bstRead8(); // serialrx_type - boardAlignment()->rollDegrees = bstRead16(); // board_align_roll - boardAlignment()->pitchDegrees = bstRead16(); // board_align_pitch - boardAlignment()->yawDegrees = bstRead16(); // board_align_yaw + boardAlignmentMutable()->rollDegrees = bstRead16(); // board_align_roll + boardAlignmentMutable()->pitchDegrees = bstRead16(); // board_align_pitch + boardAlignmentMutable()->yawDegrees = bstRead16(); // board_align_yaw - batteryConfig()->currentMeterScale = bstRead16(); - batteryConfig()->currentMeterOffset = bstRead16(); + batteryConfigMutable()->currentMeterScale = bstRead16(); + batteryConfigMutable()->currentMeterOffset = bstRead16(); break; case BST_SET_CF_SERIAL_CONFIG: { @@ -1372,7 +1372,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { { i = bstRead8(); - hsvColor_t *color = &ledStripConfig()->colors[i]; + hsvColor_t *color = &ledStripConfigMutable()->colors[i]; color->h = bstRead16(); color->s = bstRead8(); color->v = bstRead8(); @@ -1385,7 +1385,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; break; } - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i]; *ledConfig = bstRead32(); reevaluateLedConfig(); } @@ -1403,13 +1403,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) DISABLE_ARMING_FLAG(PREVENT_ARMING); break; case BST_SET_DEADBAND: - rcControlsConfig()->alt_hold_deadband = bstRead8(); - rcControlsConfig()->alt_hold_fast_change = bstRead8(); - rcControlsConfig()->deadband = bstRead8(); - rcControlsConfig()->yaw_deadband = bstRead8(); + rcControlsConfigMutable()->alt_hold_deadband = bstRead8(); + rcControlsConfigMutable()->alt_hold_fast_change = bstRead8(); + rcControlsConfigMutable()->deadband = bstRead8(); + rcControlsConfigMutable()->yaw_deadband = bstRead8(); break; case BST_SET_FC_FILTERS: - gyroConfig()->gyro_lpf = bstRead16(); + gyroConfigMutable()->gyro_lpf = bstRead16(); break; default: