From 0bca49d8c46b7de7c92b0e4b4c78347723766554 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 20 Feb 2017 15:22:56 +0000 Subject: [PATCH] Added PG config definitions 1 --- src/main/config/parameter_group.h | 39 ++++------- src/main/fc/cli.c | 61 ++++++++++++++++- src/main/fc/config.c | 37 +++++++--- src/main/fc/config.h | 3 +- src/main/fc/fc_init.c | 2 +- src/main/rx/rx.c | 109 +++++++++++++++++++++--------- src/main/rx/rx.h | 4 +- src/main/sensors/acceleration.c | 52 ++++++++++++-- src/main/sensors/barometer.c | 10 +++ src/main/sensors/battery.c | 25 +++++++ src/main/sensors/compass.c | 16 +++++ src/main/sensors/gyro.c | 2 +- 12 files changed, 281 insertions(+), 79 deletions(-) diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 33d49c6f4..75f5fb8e8 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -53,7 +53,7 @@ typedef struct pgRegistry_s { } pgRegistry_t; static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_MASK;} -static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;} +static inline uint8_t pgVersion(const pgRegistry_t* reg) {return (uint8_t)(reg->pgn >> 12);} static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;} static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;} static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;} @@ -154,6 +154,10 @@ extern const uint8_t __pg_resetdata_end[]; _reset, \ } \ /**/ +#else +#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ + _type _name ## _System +#endif #define PG_REGISTER(_type, _name, _pgn, _version) \ PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ @@ -169,6 +173,7 @@ extern const uint8_t __pg_resetdata_end[]; PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ /**/ +#ifdef USE_PARAMETER_GROUPS // Register system config array #define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \ _type _name ## _SystemArray[_size]; \ @@ -181,6 +186,10 @@ extern const uint8_t __pg_resetdata_end[]; _reset, \ } \ /**/ +#else +#define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \ + _type _name ## _SystemArray[_size] +#endif #define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version) \ PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ @@ -207,6 +216,7 @@ extern const uint8_t __pg_resetdata_end[]; _type *_name ## _ProfileCurrent; #endif +#ifdef USE_PARAMETER_GROUPS // register profile config #define PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, _reset) \ STATIC_UNIT_TESTED _type _name ## _Storage[MAX_PROFILE_COUNT]; \ @@ -220,6 +230,10 @@ extern const uint8_t __pg_resetdata_end[]; _reset, \ } \ /**/ +#else +#define PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, _reset) \ + STATIC_UNIT_TESTED _type _name ## _Storage[MAX_PROFILE_COUNT] +#endif #define PG_REGISTER_PROFILE(_type, _name, _pgn, _version) \ PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ @@ -243,29 +257,6 @@ extern const uint8_t __pg_resetdata_end[]; __VA_ARGS__ \ } \ /**/ -#else -#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ - _type _name ## _System - -#define PG_REGISTER(_type, _name, _pgn, _version) \ - PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ - /**/ - -#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \ - extern void pgResetFn_ ## _name(_type *); \ - PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \ - /**/ - -#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ - extern const _type pgResetTemplate_ ## _name; \ - PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ - /**/ -#define PG_RESET_TEMPLATE(_type, _name, ...) \ - const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \ - __VA_ARGS__ \ - } \ - /**/ -#endif const pgRegistry_t* pgFind(pgn_t pgn); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d47339371..c9dacf2fa 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -489,7 +489,66 @@ typedef struct { } __attribute__((packed)) clivalue_t; static const clivalue_t valueTable[] = { - { "dummy", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, 0, 0 } +// PG_GYRO_CONFIG + { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) }, + { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) }, + { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) }, + { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) }, + { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, + { "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) }, + +// PG_ACCELEROMETER_CONFIG + { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, + { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) }, + { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, + { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) }, + { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) }, + +// PG_COMPASS_CONFIG +#ifdef MAG + { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) }, + { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAG_HARDWARE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) }, + { "mag_declination", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_declination) }, + { "magzero_x", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[X]) }, + { "magzero_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Y]) }, + { "magzero_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Z]) }, +#endif + +// PG_BAROMETER_CONFIG +#ifdef BARO + { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) }, + { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_sample_count) }, + { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, + { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_vel) }, + { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0 , 1 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_alt) }, +#endif + +// PG_RX_CONFIG + { "mid_rc", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1200, 1700 }, PG_RX_CONFIG, offsetof(rxConfig_t, midrc) }, + { "min_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, mincheck) }, + { "max_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, maxcheck) }, + { "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) }, + { "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) }, + { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_ppm_invert) }, + { "roll_yaw_cam_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) }, +#endif + { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) }, +#ifdef SPEKTRUM_BIND + { "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) }, + { "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) }, }; #else diff --git a/src/main/fc/config.c b/src/main/fc/config.c index cb77c798b..923da65cd 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -110,13 +110,16 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; +#ifndef USE_PARAMETER_GROUPS static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { accelerometerTrims->values.pitch = 0; accelerometerTrims->values.roll = 0; accelerometerTrims->values.yaw = 0; } +#endif +#ifndef USE_PARAMETER_GROUPS static void resetCompassConfig(compassConfig_t* compassConfig) { compassConfig->mag_align = ALIGN_DEFAULT; @@ -126,6 +129,7 @@ static void resetCompassConfig(compassConfig_t* compassConfig) compassConfig->interruptTag = IO_TAG_NONE; #endif } +#endif static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { @@ -219,6 +223,7 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) #endif #ifdef BARO +#ifndef USE_PARAMETER_GROUPS void resetBarometerConfig(barometerConfig_t *barometerConfig) { barometerConfig->baro_sample_count = 21; @@ -227,6 +232,7 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_alt = 0.965f; } #endif +#endif #ifdef LED_STRIP void resetLedStripConfig(ledStripConfig_t *ledStripConfig) @@ -422,6 +428,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetBatteryConfig(batteryConfig_t *batteryConfig) { batteryConfig->vbatscale = VBAT_SCALE_DEFAULT; @@ -441,6 +448,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->useConsumptionAlerts = false; batteryConfig->consumptionWarningPercentage = 10; } +#endif // Default pin (NONE). // XXX Does this mess belong here??? @@ -789,6 +797,7 @@ void createDefaultConfig(master_t *config) config->current_profile_index = 0; // default profile config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000 +#ifndef USE_PARAMETER_GROUPS config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X config->gyroConfig.gyro_sync_denom = 8; @@ -806,30 +815,39 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyro_soft_notch_cutoff_1 = 300; config->gyroConfig.gyro_soft_notch_hz_2 = 200; config->gyroConfig.gyro_soft_notch_cutoff_2 = 100; + config->gyroConfig.gyro_align = ALIGN_DEFAULT; + config->gyroConfig.gyroMovementCalibrationThreshold = 48; +#endif config->systemConfig.debug_mode = DEBUG_MODE; config->task_statistics = true; - resetAccelerometerTrims(&config->accelerometerConfig.accZero); - config->gyroConfig.gyro_align = ALIGN_DEFAULT; - config->accelerometerConfig.acc_align = ALIGN_DEFAULT; +#ifndef USE_PARAMETER_GROUPS resetCompassConfig(&config->compassConfig); +#endif +#ifndef USE_PARAMETER_GROUPS + resetAccelerometerTrims(&config->accelerometerConfig.accZero); + config->accelerometerConfig.acc_align = ALIGN_DEFAULT; + config->accelerometerConfig.acc_hardware = ACC_DEFAULT; // default/autodetect + resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims); + config->accelerometerConfig.acc_lpf_hz = 10.0f; +#endif config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; - config->accelerometerConfig.acc_hardware = ACC_DEFAULT; // default/autodetect config->rcControlsConfig.yaw_control_direction = 1; - config->gyroConfig.gyroMovementCalibrationThreshold = 48; // xxx_hardware: 0:default/autodetect, 1: disable config->compassConfig.mag_hardware = 1; config->barometerConfig.baro_hardware = 1; +#ifndef USE_PARAMETER_GROUPS resetBatteryConfig(&config->batteryConfig); +#endif #if defined(USE_PWM) || defined(USE_PPM) resetPpmConfig(&config->ppmConfig); @@ -857,6 +875,7 @@ void createDefaultConfig(master_t *config) resetsdcardConfig(&config->sdcardConfig); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef SERIALRX_PROVIDER config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else @@ -890,6 +909,7 @@ void createDefaultConfig(master_t *config) config->rxConfig.airModeActivateThreshold = 1350; resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); +#endif #ifdef USE_PWM config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; @@ -928,17 +948,16 @@ void createDefaultConfig(master_t *config) resetProfile(&config->profile[0]); - resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims); - config->compassConfig.mag_declination = 0; - config->accelerometerConfig.acc_lpf_hz = 10.0f; config->imuConfig.accDeadband.xy = 40; config->imuConfig.accDeadband.z = 40; config->imuConfig.acc_unarmedcal = 1; #ifdef BARO +#ifndef USE_PARAMETER_GROUPS resetBarometerConfig(&config->barometerConfig); +#endif #endif // Radio @@ -1145,8 +1164,6 @@ void validateAndFixConfig(void) } #endif - useRxConfig(&masterConfig.rxConfig); - serialConfig_t *serialConfig = &masterConfig.serialConfig; if (!isSerialConfigValid(serialConfig)) { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0e49440cb..ed958dfd9 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -26,6 +26,7 @@ #include "drivers/flash.h" #include "drivers/rx_pwm.h" #include "drivers/sdcard.h" +#include "drivers/serial.h" #include "drivers/sound_beeper.h" #include "drivers/vcd.h" @@ -81,7 +82,7 @@ PG_DECLARE(ppmConfig_t, ppmConfig); PG_DECLARE(pwmConfig_t, pwmConfig); PG_DECLARE(vcdProfile_t, vcdProfile); PG_DECLARE(sdcardConfig_t, sdcardConfig); - +PG_DECLARE(serialPinConfig_t, serialPinConfig); /*typedef struct beeperConfig_s { uint32_t beeper_off_flags; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f57f050f5..396ac2c6f 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -446,7 +446,7 @@ void init(void) failsafeInit(); - rxInit(rxConfig(), modeActivationConditions(0)); + rxInit(); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 309f22082..028ae09ac 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -94,6 +94,68 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + +#ifndef RX_SPI_DEFAULT_PROTOCOL +#define RX_SPI_DEFAULT_PROTOCOL 0 +#endif +#ifndef SERIALRX_PROVIDER +#define SERIALRX_PROVIDER 0 +#endif + +PG_RESET_TEMPLATE(rxConfig_t, rxConfig, + .halfDuplex = 0, + .serialrx_provider = SERIALRX_PROVIDER, + .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, + .sbus_inversion = 1, + .spektrum_sat_bind = 0, + .spektrum_sat_bind_autoreset = 1, + .midrc = 1500, + .mincheck = 1100, + .maxcheck = 1900, + .rx_min_usec = 885, // any of first 4 channels below this value will trigger rx loss detection + .rx_max_usec = 2115, // any of first 4 channels above this value will trigger rx loss detection + .rssi_channel = 0, + .rssi_scale = RSSI_SCALE_DEFAULT, + .rssi_ppm_invert = 0, + .rcInterpolation = RC_SMOOTHING_AUTO, + .rcInterpolationChannels = 0, + .rcInterpolationInterval = 19, + .fpvCamAngleDegrees = 0, + .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT, + .airModeActivateThreshold = 1350 +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs, PG_RX_CHANNEL_RANGE_CONFIG, 0); +void pgResetFn_rxChannelRangeConfigs(rxChannelRangeConfig_t *rxChannelRangeConfigs) +{ + // set default calibration to full range and 1:1 mapping + for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + rxChannelRangeConfigs[i].min = PWM_RANGE_MIN; + rxChannelRangeConfigs[i].max = PWM_RANGE_MAX; + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0); +void pgResetFn_rxFailsafeChannelConfigs(rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs) +{ + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + rxFailsafeChannelConfigs[i].mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; + rxFailsafeChannelConfigs[i].step = (i == THROTTLE) + ? CHANNEL_VALUE_TO_RXFAIL_STEP(rxConfig()->rx_min_usec) + : CHANNEL_VALUE_TO_RXFAIL_STEP(rxConfig()->midrc); + } +} + +void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig) { + // set default calibration to full range and 1:1 mapping + for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + rxChannelRangeConfig->min = PWM_RANGE_MIN; + rxChannelRangeConfig->max = PWM_RANGE_MAX; + rxChannelRangeConfig++; + } +} + static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { UNUSED(rxRuntimeConfig); @@ -107,11 +169,6 @@ static uint8_t nullFrameStatus(void) return RX_FRAME_PENDING; } -void useRxConfig(const rxConfig_t *rxConfigToUse) -{ - (void)(rxConfigToUse); -} - #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels static uint8_t validFlightChannelMask; @@ -140,15 +197,6 @@ STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, bool valid) } } -void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig) { - // set default calibration to full range and 1:1 mapping - for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfig->min = PWM_RANGE_MIN; - rxChannelRangeConfig->max = PWM_RANGE_MAX; - rxChannelRangeConfig++; - } -} - #ifdef SERIAL_RX bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { @@ -205,9 +253,8 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions) +void rxInit(void) { - useRxConfig(initialRxConfig); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; @@ -222,7 +269,7 @@ void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t * // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i]; + const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i); if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { // ARM switch is defined, determine an OFF value uint16_t value; @@ -404,20 +451,21 @@ static uint16_t getRxfailValue(uint8_t channel) } } -STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig_t range) +STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range) { // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT if (sample == PPM_RCVR_TIMEOUT) { return PPM_RCVR_TIMEOUT; } - sample = scaleRange(sample, range.min, range.max, PWM_RANGE_MIN, PWM_RANGE_MAX); + sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX); sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX); return sample; } -static uint8_t getRxChannelCount(void) { +static uint8_t getRxChannelCount(void) +{ static uint8_t maxChannelsAllowed; if (!maxChannelsAllowed) { @@ -444,7 +492,7 @@ static void readRxChannelsApplyRanges(void) // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]); + sample = applyRxChannelRangeConfiguraton(sample, &rxConfig()->channelRanges[channel]); } rcRaw[channel] = sample; @@ -536,12 +584,11 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) void parseRcChannels(const char *input, rxConfig_t *rxConfig) { - const char *c, *s; - - for (c = input; *c; c++) { - s = strchr(rcChannelLetters, *c); - if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS)) + for (const char *c = input; *c; c++) { + const char *s = strchr(rcChannelLetters, *c); + if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS)) { rxConfig->rcmap[s - rcChannelLetters] = c - input; + } } } @@ -577,17 +624,15 @@ static void updateRSSIADC(timeUs_t currentTimeUs) } rssiUpdateAt = currentTimeUs + DELAY_50_HZ; - int16_t adcRssiMean = 0; - uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; + const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); + const uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; adcRssiSamples[adcRssiSampleIndex] = rssiPercentage; - uint8_t sampleIndex; - - for (sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) { + int16_t adcRssiMean = 0; + for (int sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) { adcRssiMean += adcRssiSamples[sampleIndex]; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6e588acf2..7977fd7ba 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -162,9 +162,7 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount -struct modeActivationCondition_s; -void rxInit(const rxConfig_t *rxConfig, const struct modeActivationCondition_s *modeActivationConditions); -void useRxConfig(const rxConfig_t *rxConfigToUse); +void rxInit(void); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index c664aa2b5..0136e07d8 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -29,6 +29,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" @@ -80,6 +81,51 @@ static flightDynamicsTrims_t *accelerationTrims; static uint16_t accLpfCutHz = 0; static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); + +void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) +{ +#ifdef USE_PARAMETER_GROUPS + RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims, + .values.roll = 0, + .values.pitch = 0, + ); +#else + rollAndPitchTrims->values.roll = 0; + rollAndPitchTrims->values.pitch = 0; +#endif +} + +void accResetRollAndPitchTrims(void) +{ + resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims); +} + +static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero) +{ + accZero->values.roll = 0; + accZero->values.pitch = 0; + accZero->values.yaw = 0; +} + +void accResetFlightDynamicsTrims(void) +{ + resetFlightDynamicsTrims(&accelerometerConfigMutable()->accZero); +} + +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) +{ + RESET_CONFIG_2(accelerometerConfig_t, instance, + .acc_lpf_hz = 15, + .acc_align = ALIGN_DEFAULT, + .acc_hardware = ACC_DEFAULT + ); + resetRollAndPitchTrims(&instance->accelerometerTrims); + resetFlightDynamicsTrims(&instance->accZero); +} +#endif + bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; @@ -295,12 +341,6 @@ static bool isOnFirstAccelerationCalibrationCycle(void) return calibratingA == CALIBRATING_ACC_CYCLES; } -void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) -{ - rollAndPitchTrims->values.roll = 0; - rollAndPitchTrims->values.pitch = 0; -} - static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index d5a130df4..c39ad80ed 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -44,6 +44,16 @@ baro_t baro; // barometer access functions +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); + +PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, + .baro_hardware = 1, + .baro_sample_count = 21, + .baro_noise_lpf = 0.6f, + .baro_cf_vel = 0.985f, + .baro_cf_alt = 0.965f +); + #ifdef BARO static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 6a780536a..9a08c2a0c 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -68,6 +68,31 @@ int32_t mAhDrawn = 0; // milliampere hours drawn from the battery static batteryState_e vBatState; static batteryState_e consumptionState; +PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + +#ifndef CURRENT_METER_SCALE_DEFAULT +#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A) +#endif + +PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, + .vbatscale = VBAT_SCALE_DEFAULT, + .vbatresdivval = VBAT_RESDIVVAL_DEFAULT, + .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT, + .vbatmaxcellvoltage = 43, + .vbatmincellvoltage = 33, + .vbatwarningcellvoltage = 35, + .vbathysteresis = 1, + .batteryMeterType = BATTERY_SENSOR_ADC, + .currentMeterOffset = 0, + .currentMeterScale = CURRENT_METER_SCALE_DEFAULT, + .batteryCapacity = 0, + .currentMeterType = CURRENT_SENSOR_ADC, + .batterynotpresentlevel = 55, // VBAT below 5.5 V will be igonored + .useVBatAlerts = true, + .useConsumptionAlerts = false, + .consumptionWarningPercentage = 10 +); + static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 33393463c..fe31eb3f1 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -47,6 +47,22 @@ magDev_t magDev; mag_t mag; // mag access functions +#ifdef MAG_INT_EXTI +#define COMPASS_INTERRUPT_TAG IO_TAG(MAG_INT_EXTI) +#else +#define COMPASS_INTERRUPT_TAG IO_TAG_NONE +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0); + +PG_RESET_TEMPLATE(compassConfig_t, compassConfig, + .mag_align = ALIGN_DEFAULT, + // xxx_hardware: 0:default/autodetect, 1: disable + .mag_hardware = 1, + .mag_declination = 0, + .interruptTag = COMPASS_INTERRUPT_TAG +); + #ifdef MAG static int16_t magADCRaw[XYZ_AXIS_COUNT]; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c06b4b086..e3d368a86 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -106,7 +106,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_hz_2 = 200, .gyro_soft_notch_cutoff_2 = 100, .gyro_align = ALIGN_DEFAULT, - .gyroMovementCalibrationThreshold = 32 + .gyroMovementCalibrationThreshold = 48 ); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)