From c92679f45439cf4d69d431554c17a8176ee458d7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 25 Feb 2017 17:35:00 +0000 Subject: [PATCH] Added PG config definitions 4 --- src/main/config/parameter_group_ids.h | 6 +++-- src/main/fc/config.c | 31 +++++++++++++++------- src/main/fc/fc_core.c | 7 +++++ src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_adjustments.c | 2 ++ src/main/fc/rc_controls.c | 11 ++++++++ src/main/flight/imu.c | 11 ++++++++ src/main/flight/mixer.c | 38 +++++++++++++++++++++++++++ src/main/flight/mixer.h | 2 ++ src/main/flight/pid.c | 13 +++++++++ 10 files changed, 110 insertions(+), 13 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 0d6d189f63..b42d360939 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -83,8 +83,10 @@ #define PG_BETAFLIGHT_START 500 #define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500 #define PG_OSD_CONFIG 501 -#define PG_BEEPER_CONFIG 5002 -#define PG_BETAFLIGHT_END 1002 +#define PG_BEEPER_CONFIG 502 +#define PG_PID_CONFIG 503 +#define PG_STATUS_LED_CONFIG 504 +#define PG_BETAFLIGHT_END 504 // OSD configuration (subject to change) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 00795d43e4..1cc87d1dbc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -104,9 +104,6 @@ #define RX_SPI_DEFAULT_PROTOCOL 0 #endif -#define BRUSHED_MOTORS_PWM_RATE 16000 -#define BRUSHLESS_MOTORS_PWM_RATE 480 - PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, @@ -125,6 +122,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; @@ -294,6 +292,7 @@ void resetServoConfig(servoConfig_t *servoConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS @@ -328,6 +327,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) } } } +#endif #ifdef SONAR void resetSonarConfig(sonarConfig_t *sonarConfig) @@ -676,6 +676,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->deadband = 0; @@ -683,6 +684,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) rcControlsConfig->alt_hold_deadband = 40; rcControlsConfig->alt_hold_fast_change = 1; } +#endif #ifndef USE_PARAMETER_GROUPS void resetMixerConfig(mixerConfig_t *mixerConfig) @@ -711,7 +713,11 @@ void resetDisplayPortProfile(displayPortProfile_t *pDisplayPortProfile) pDisplayPortProfile->rowAdjust = 0; } +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) +#else void resetStatusLedConfig(statusLedConfig_t *statusLedConfig) +#endif { for (int i = 0; i < LED_NUMBER; i++) { statusLedConfig->ledTags[i] = IO_TAG_NONE; @@ -832,8 +838,14 @@ void createDefaultConfig(master_t *config) #endif +#ifndef USE_PARAMETER_GROUPS config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000 + config->imuConfig.small_angle = 25; + config->imuConfig.accDeadband.xy = 40; + config->imuConfig.accDeadband.z = 40; + config->imuConfig.acc_unarmedcal = 1; +#endif #ifndef USE_PARAMETER_GROUPS config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X @@ -888,9 +900,11 @@ void createDefaultConfig(master_t *config) resetPwmConfig(&config->pwmConfig); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef TELEMETRY resetTelemetryConfig(&config->telemetryConfig); #endif +#endif #ifdef USE_ADC resetAdcConfig(&config->adcConfig); @@ -955,15 +969,14 @@ void createDefaultConfig(master_t *config) config->armingConfig.auto_disarm_delay = 5; #endif - config->imuConfig.small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo #ifndef USE_PARAMETER_GROUPS resetMixerConfig(&config->mixerConfig); -#endif resetMotorConfig(&config->motorConfig); +#endif #ifdef USE_SERVOS resetServoConfig(&config->servoConfig); #endif @@ -993,10 +1006,6 @@ void createDefaultConfig(master_t *config) config->compassConfig.mag_declination = 0; - config->imuConfig.accDeadband.xy = 40; - config->imuConfig.accDeadband.z = 40; - config->imuConfig.acc_unarmedcal = 1; - #ifdef BARO #ifndef USE_PARAMETER_GROUPS resetBarometerConfig(&config->barometerConfig); @@ -1010,12 +1019,12 @@ void createDefaultConfig(master_t *config) parseRcChannels("AETR1234", &config->rxConfig); #endif +#ifndef USE_PARAMETER_GROUPS resetRcControlsConfig(&config->rcControlsConfig); config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv -#ifndef USE_PARAMETER_GROUPS // Failsafe Variables config->failsafeConfig.failsafe_delay = 10; // 1sec config->failsafeConfig.failsafe_off_delay = 10; // 1sec @@ -1095,7 +1104,9 @@ void createDefaultConfig(master_t *config) resetFlashConfig(&config->flashConfig); #endif +#ifndef USE_PARAMETER_GROUPS resetStatusLedConfig(&config->statusLedConfig); +#endif /* merely to force a reset if the person inadvertently flashes the wrong target */ strncpy(config->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3a561ad83b..e4ce797d7c 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -102,6 +102,13 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m bool isRXDataNew; static bool armingCalibrationWasInitialised; +PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0); + +PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, + .throttle_correction_value = 0, // could 10 with althold or 40 for fpv + .throttle_correction_angle = 800 // could be 80.0 deg with atlhold or 45.0 for fpv +); + void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 3a76687c4a..357c76abf4 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -165,7 +165,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) static void taskUpdateCompass(timeUs_t currentTimeUs) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTimeUs, &compassConfig()->magZero); + compassUpdate(currentTimeUs, &compassConfigMutable()->magZero); } } #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 31c365a28a..db4669d7ed 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -50,6 +50,8 @@ #include "rx/rx.h" +PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0); + static pidProfile_t *pidProfile; static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 909bc335db..58eecc4439 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -70,6 +70,15 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); + +PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, + .deadband = 0, + .yaw_deadband = 0, + .alt_hold_deadband = 40, + .alt_hold_fast_change = 1 +); + PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, @@ -78,6 +87,8 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .auto_disarm_delay = 5 ); +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); + bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 97da5647b3..9fc7c08e29 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -76,6 +76,17 @@ static float rMat[3][3]; attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 +PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); + +PG_RESET_TEMPLATE(imuConfig_t, imuConfig, + .dcm_kp = 2500, // 1.0 * 10000 + .dcm_ki = 0, // 0.003 * 10000 + .small_angle = 25, + .accDeadband.xy = 40, + .accDeadband.z = 40, + .acc_unarmedcal = 1 +); + STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) { float q1q1 = sq(q1); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ad75905ca9..0c20a275ec 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -34,6 +34,7 @@ #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_esc_detect.h" #include "io/motors.h" @@ -69,6 +70,43 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .yaw_motor_direction = 1, ); +PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); + +void pgResetFn_motorConfig(motorConfig_t *motorConfig) +{ +#ifdef BRUSHED_MOTORS + motorConfig->minthrottle = 1000; + motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; + motorConfig->dev.useUnsyncedPwm = true; +#else +#ifdef BRUSHED_ESC_AUTODETECT + if (hardwareMotorType == MOTOR_BRUSHED) { + motorConfig->minthrottle = 1000; + motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; + motorConfig->dev.useUnsyncedPwm = true; + } else +#endif + { + motorConfig->minthrottle = 1070; + motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125; + } +#endif + motorConfig->maxthrottle = 2000; + motorConfig->mincommand = 1000; + motorConfig->digitalIdleOffsetPercent = 4.5f; + + int motorIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { + motorConfig->dev.ioTags[motorIndex] = timerHardware[i].tag; + motorIndex++; + } + } +} + #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8047d945fc..831b3dd978 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -22,6 +22,8 @@ #include "drivers/pwm_output.h" #define QUAD_MOTOR_COUNT 4 +#define BRUSHED_MOTORS_PWM_RATE 16000 +#define BRUSHLESS_MOTORS_PWM_RATE 480 /* DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 04937c42be..3a8b7c7222 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -58,6 +58,19 @@ static float previousGyroIf[3]; static float dT; +PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 0); + +#ifdef STM32F10X +#define PID_PROCESS_DENOM_DEFAULT 1 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define PID_PROCESS_DENOM_DEFAULT 4 +#else +#define PID_PROCESS_DENOM_DEFAULT 2 +#endif +PG_RESET_TEMPLATE(pidConfig_t, pidConfig, + .pid_process_denom = PID_PROCESS_DENOM_DEFAULT +); + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime;