diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 62c8c6181a..d8fac13776 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -20,7 +20,7 @@ #include #include -#define EEPROM_CONF_VERSION 154 +#define EEPROM_CONF_VERSION 155 bool isEEPROMContentValid(void); bool loadEEPROM(void); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 4a0e9b6a48..ffe33a3d2a 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -41,7 +41,9 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SPI_MOSI", "I2C_SCL", "I2C_SDA", + "SDCARD", "SDCARD_CS", + "SDCARD_DETECT", "FLASH_CS", "BARO_CS", "MPU_CS", @@ -54,7 +56,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "USB_DETECT", "BEEPER", "OSD", - "SDCARD_DETECT", "RX_BIND", "INVERTER", "LED_STRIP", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index f5ba5f6a59..4b566efcfc 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -41,7 +41,9 @@ typedef enum { OWNER_SPI_MOSI, OWNER_I2C_SCL, OWNER_I2C_SDA, + OWNER_SDCARD, OWNER_SDCARD_CS, + OWNER_SDCARD_DETECT, OWNER_FLASH_CS, OWNER_BARO_CS, OWNER_MPU_CS, @@ -54,7 +56,6 @@ typedef enum { OWNER_USB_DETECT, OWNER_BEEPER, OWNER_OSD, - OWNER_SDCARD_DETECT, OWNER_RX_BIND, OWNER_INVERTER, OWNER_LED_STRIP, diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 66aafc25b2..02441b2c66 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -24,6 +24,7 @@ #include "nvic.h" #include "io.h" +#include "dma.h" #include "bus_spi.h" #include "system.h" @@ -551,6 +552,9 @@ void sdcard_init(bool useDMA) { #ifdef SDCARD_DMA_CHANNEL_TX useDMAForTx = useDMA; + if (useDMAForTx) { + dmaInit(dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX), OWNER_SDCARD, 0); + } #else // DMA is not available (void) useDMA; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5a207ce557..a1dc251673 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -521,7 +521,7 @@ static const clivalue_t valueTable[] = { { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, #endif { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, - { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, 13 } }, + { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, MAX_AUX_CHANNELS } }, { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -717,7 +717,6 @@ static const clivalue_t valueTable[] = { { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, - { "anti_gravity_rate_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, @@ -725,7 +724,7 @@ static const clivalue_t valueTable[] = { { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, - { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, + { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, MAX_PID_PROCESS_DENOM } }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 86b2300b5c..01900831ca 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -60,6 +60,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/fc_rc.h" #include "fc/runtime_config.h" #include "flight/altitudehold.h" @@ -101,11 +102,7 @@ #endif #define BRUSHED_MOTORS_PWM_RATE 16000 -#ifdef STM32F4 -#define BRUSHLESS_MOTORS_PWM_RATE 2000 -#else -#define BRUSHLESS_MOTORS_PWM_RATE 400 -#endif +#define BRUSHLESS_MOTORS_PWM_RATE 480 master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; @@ -179,13 +176,12 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->levelAngleLimit = 55; pidProfile->levelSensitivity = 55; - pidProfile->setpointRelaxRatio = 30; + pidProfile->setpointRelaxRatio = 25; pidProfile->dtermSetpointWeight = 190; pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 350; - pidProfile->itermAcceleratorGain = 2.0f; - pidProfile->itermAcceleratorRateLimit = 80; + pidProfile->itermAcceleratorGain = 1.0f; } void resetProfile(profile_t *profile) @@ -888,6 +884,8 @@ void resetConfigs(void) void activateConfig(void) { + generateThrottleCurve(); + resetAdjustmentStates(); useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); @@ -924,6 +922,10 @@ void validateAndFixConfig(void) motorConfigMutable()->mincommand = 1000; } + if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) { + motorConfig()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; + } + if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { featureSet(DEFAULT_RX_FEATURE); } @@ -1072,7 +1074,7 @@ void validateAndFixGyroConfig(void) float motorUpdateRestriction; switch(motorConfig()->motorPwmProtocol) { case (PWM_TYPE_STANDARD): - motorUpdateRestriction = 0.002f; + motorUpdateRestriction = 1.0f/BRUSHLESS_MOTORS_PWM_RATE; break; case (PWM_TYPE_ONESHOT125): motorUpdateRestriction = 0.0005f; @@ -1092,11 +1094,13 @@ void validateAndFixGyroConfig(void) motorUpdateRestriction = 0.00003125f; } - if(pidLooptime < motorUpdateRestriction) - pidConfigMutable()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom); + if (pidLooptime < motorUpdateRestriction) { + const uint8_t maxPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); + pidConfigMutable()->pid_process_denom = MIN(pidConfigMutable()->pid_process_denom, maxPidProcessDenom); + } // Prevent overriding the max rate of motors - if(motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED)) { + if (motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED) && motorConfig()->motorPwmProtocol != PWM_TYPE_STANDARD) { uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); if(motorConfig()->motorPwmRate > maxEscRate) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b8a7d5aeb5..d882b9eb5d 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -59,6 +59,7 @@ #include "fc/config.h" #include "fc/fc_core.h" #include "fc/fc_msp.h" +#include "fc/fc_rc.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -1404,6 +1405,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (dataSize >= 12) { currentControlRateProfile->rcYawRate8 = sbufReadU8(src); } + generateThrottleCurve(); } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 950054e3e3..9787a03877 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -63,13 +63,39 @@ float getThrottlePIDAttenuation(void) { return throttlePIDAttenuation; } +#define THROTTLE_LOOKUP_LENGTH 12 +static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE + +void generateThrottleCurve(void) +{ + uint8_t i; + + for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { + int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; + uint8_t y = 1; + if (tmp > 0) + y = 100 - currentControlRateProfile->thrMid8; + if (tmp < 0) + y = currentControlRateProfile->thrMid8; + lookupThrottleRC[i] = 10 * currentControlRateProfile->thrMid8 + tmp * (100 - currentControlRateProfile->thrExpo8 + (int32_t) currentControlRateProfile->thrExpo8 * (tmp * tmp) / (y * y)) / 10; + lookupThrottleRC[i] = PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + } +} + +int16_t rcLookupThrottle(int32_t tmp) +{ + const int32_t tmp2 = tmp / 100; + // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] + return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; +} + #define SETPOINT_RATE_LIMIT 1998.0f #define RC_RATE_INCREMENTAL 14.54f -void calculateSetpointRate(int axis, int16_t rc) { - float angleRate, rcRate, rcSuperfactor, rcCommandf; +static void calculateSetpointRate(int axis) +{ uint8_t rcExpo; - + float rcRate; if (axis != YAW) { rcExpo = currentControlRateProfile->rcExpo8; rcRate = currentControlRateProfile->rcRate8 / 100.0f; @@ -77,21 +103,23 @@ void calculateSetpointRate(int axis, int16_t rc) { rcExpo = currentControlRateProfile->rcYawExpo8; rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; } - - if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); - rcCommandf = rc / 500.0f; - rcDeflection[axis] = rcCommandf; - rcDeflectionAbs[axis] = ABS(rcCommandf); - - if (rcExpo) { - float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof); + if (rcRate > 2.0f) { + rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); } - angleRate = 200.0f * rcRate * rcCommandf; + float rcCommandf = rcCommand[axis] / 500.0f; + rcDeflection[axis] = rcCommandf; + const float rcCommandfAbs = ABS(rcCommandf); + rcDeflectionAbs[axis] = rcCommandfAbs; + if (rcExpo) { + const float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1-expof); + } + + float angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; } @@ -100,7 +128,7 @@ void calculateSetpointRate(int axis, int16_t rc) { setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } -void scaleRcCommandToFpvCamAngle(void) { +static void scaleRcCommandToFpvCamAngle(void) { //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed static uint8_t lastFpvCamAngleDegrees = 0; static float cosFactor = 1.0; @@ -121,7 +149,7 @@ void scaleRcCommandToFpvCamAngle(void) { #define THROTTLE_BUFFER_MAX 20 #define THROTTLE_DELTA_MS 100 - void checkForThrottleErrorResetState(uint16_t rxRefreshRate) { + static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) { static int index; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; const int rxRefreshRateMs = rxRefreshRate / 1000; @@ -153,7 +181,8 @@ void processRcCommand(void) if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - checkForThrottleErrorResetState(currentRxRefreshRate); + if (currentProfile->pidProfile.itermAcceleratorGain > 1.0f) + checkForThrottleErrorResetState(currentRxRefreshRate); } if (rxConfig()->rcInterpolation || flightModeFlags) { @@ -208,7 +237,7 @@ void processRcCommand(void) readyToCalculateRateAxisCnt = FD_YAW; for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) - calculateSetpointRate(axis, rcCommand[axis]); + calculateSetpointRate(axis); // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) @@ -267,13 +296,7 @@ void updateRcCommands(void) tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); } - if (currentControlRateProfile->thrExpo8) { - float expof = currentControlRateProfile->thrExpo8 / 100.0f; - float tmpf = (tmp / (PWM_RANGE_MAX - PWM_RANGE_MIN)); - tmp = lrintf(tmp * sq(tmpf) * expof + tmp * (1-expof)); - } - - rcCommand[THROTTLE] = tmp + (PWM_RANGE_MAX - PWM_RANGE_MIN); + rcLookupThrottle(tmp); if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 34114b053c..7c2f378645 100755 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -16,12 +16,6 @@ */ #pragma once -#include - -void calculateSetpointRate(int axis, int16_t rc); -void scaleRcCommandToFpvCamAngle(void); -void checkForThrottleErrorResetState(uint16_t rxRefreshRate); -void checkForThrottleErrorResetState(uint16_t rxRefreshRate); void processRcCommand(void); float getSetpointRate(int axis); float getRcDeflection(int axis); @@ -29,3 +23,4 @@ float getRcDeflectionAbs(int axis); float getThrottlePIDAttenuation(void); void updateRcCommands(void); void resetYawAxis(void); +void generateThrottleCurve(void); diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 69c04174b9..62669bb66e 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -45,7 +45,7 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" +#include "fc/fc_rc.h" #include "fc/config.h" #include "rx/rx.h" @@ -254,6 +254,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_THROTTLE_EXPO: newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c controlRateConfig->thrExpo8 = newValue; + generateThrottleCurve(); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index d6108b5482..e7784610e3 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -39,6 +39,7 @@ #include "fc/config.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" +#include "fc/fc_rc.h" #include "fc/runtime_config.h" #include "io/gps.h" @@ -66,7 +67,6 @@ static pidProfile_t *pidProfile; static bool isUsingSticksToArm = true; int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -int16_t rcCommandSmooth[4]; uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index ce92e6b477..bd23a30752 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -166,7 +166,6 @@ typedef struct controlRateConfig_s { //!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); extern int16_t rcCommand[4]; -extern int16_t rcCommandSmooth[4]; typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c deleted file mode 100644 index 84b437b221..0000000000 --- a/src/main/fc/rc_curves.c +++ /dev/null @@ -1,60 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#include "config/feature.h" - -#include "io/motors.h" - -#include "fc/config.h" -#include "fc/rc_curves.h" -#include "fc/rc_controls.h" - -#include "rx/rx.h" - - -#define THROTTLE_LOOKUP_LENGTH 12 -static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE - -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig) -{ - uint8_t i; - uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle); - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - controlRateConfig->thrMid8; - uint8_t y = 1; - if (tmp > 0) - y = 100 - controlRateConfig->thrMid8; - if (tmp < 0) - y = controlRateConfig->thrMid8; - lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = minThrottle + (int32_t) (motorConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] - } -} - -int16_t rcLookupThrottle(int32_t tmp) -{ - const int32_t tmp2 = tmp / 100; - // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] - return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; -} - diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b7106432fc..fa59064ecc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -155,7 +155,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) static float Kp[3], Ki[3], Kd[3], maxVelocity[3]; static float relaxFactor; static float dtermSetpointWeight; -static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -172,7 +172,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); - itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit; } static float calcHorizonLevelStrength(void) { @@ -258,12 +257,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an float ITerm = previousGyroIf[axis]; if (motorMixRange < 1.0f) { // Only increase ITerm if motor output is not saturated - float ITermDelta = Ki[axis] * errorRate * dT * dynKi; - if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) { - // ITerm will only be accelerated below steady rate threshold - ITermDelta *= itermAccelerator; - } - ITerm += ITermDelta; + ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator; previousGyroIf[axis] = ITerm; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 1e093cdfc1..0d68827eed 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,14 +19,13 @@ #include -#include "config/parameter_group.h" - -#define PID_CONTROLLER_BETAFLIGHT 1 -#define PID_MIXER_SCALING 1000.0f -#define PID_SERVO_MIXER_SCALING 0.7f -#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter -#define PIDSUM_LIMIT 0.5f +#define MAX_PID_PROCESS_DENOM 16 +#define PID_CONTROLLER_BETAFLIGHT 1 +#define PID_MIXER_SCALING 1000.0f +#define PID_SERVO_MIXER_SCALING 0.7f +#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter +#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter +#define PIDSUM_LIMIT 0.5f // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float #define PTERM_SCALE 0.032029f @@ -80,7 +79,6 @@ typedef struct pidProfile_s { // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit - uint16_t itermAcceleratorRateLimit; // Setpointrate limit for iterm accelerator to operate within uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 34da6e97fa..5021b0a928 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -89,8 +89,14 @@ uint16_t trampCurConfigPower = 0; // Configured transmitting power int16_t trampCurTemp = 0; uint8_t trampCurPitmode = 0; +// Maximum number of requests sent to try a config change +#define TRAMP_MAX_RETRIES 2 + uint32_t trampConfFreq = 0; +uint8_t trampFreqRetries = 0; + uint16_t trampConfPower = 0; +uint8_t trampPowerRetries = 0; #ifdef CMS static void trampCmsUpdateStatusString(void); // Forward @@ -157,6 +163,13 @@ bool trampCommitChanges() return false; trampStatus = TRAMP_STATUS_SET_FREQ_PW; + + if(trampConfFreq != trampCurFreq) + trampFreqRetries = TRAMP_MAX_RETRIES; + + if(trampConfPower != trampCurPower) + trampPowerRetries = TRAMP_MAX_RETRIES; + return true; } @@ -194,6 +207,9 @@ char trampHandleResponse(void) trampCurPitmode = trampRespBuffer[7]; trampCurPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); vtx58_Freq2Bandchan(trampCurFreq, &trampCurBand, &trampCurChan); + + if(trampConfFreq == 0) trampConfFreq = trampCurFreq; + if(trampConfPower == 0) trampConfPower = trampCurPower; return 'v'; } @@ -361,15 +377,17 @@ void vtxTrampProcess(uint32_t currentTimeUs) case TRAMP_STATUS_SET_FREQ_PW: { bool done = true; - if (trampConfFreq != trampCurFreq) { + if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) { trampSendFreq(trampConfFreq); + trampFreqRetries--; #ifdef TRAMP_DEBUG debugFreqReqCounter++; #endif done = false; } - else if (trampConfPower != trampCurConfigPower) { + else if (trampConfPower && trampPowerRetries && (trampConfPower != trampCurConfigPower)) { trampSendRFPower(trampConfPower); + trampPowerRetries--; #ifdef TRAMP_DEBUG debugPowReqCounter++; #endif @@ -385,6 +403,10 @@ void vtxTrampProcess(uint32_t currentTimeUs) else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; + // reset configuration value in case it failed (no more retries) + trampConfFreq = trampCurFreq; + trampConfPower = trampCurPower; + trampFreqRetries = trampPowerRetries = 0; } } break; diff --git a/src/main/fc/rc_curves.h b/src/main/target/BETAFLIGHTF3/config.c old mode 100644 new mode 100755 similarity index 75% rename from src/main/fc/rc_curves.h rename to src/main/target/BETAFLIGHTF3/config.c index c13851969e..e7b417c39e --- a/src/main/fc/rc_curves.h +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -15,11 +15,16 @@ * along with Cleanflight. If not, see . */ -#pragma once +#include +#include -struct controlRateConfig_s; -struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); +#include -int16_t rcLookupThrottle(int32_t tmp); +#include "config/config_master.h" +#include "config/feature.h" + +void targetConfiguration(master_t *config) +{ + config->batteryConfig.currentMeterScale = 235; +} diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index a81ea68ea0..cd94249f4d 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -21,6 +21,7 @@ #define TARGET_BOARD_IDENTIFIER "BFF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE +#define TARGET_CONFIG #define BEEPER PC15 #define BEEPER_INVERTED @@ -122,8 +123,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY ) #define SPEKTRUM_BIND #define BIND_PIN UART2_RX_PIN diff --git a/src/main/target/common.h b/src/main/target/common.h index 313a73b635..2117ca1478 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -75,7 +75,7 @@ #define USE_PPM #if defined(STM_FAST_TARGET) -#define MAX_AUX_CHANNELS 99 +#define MAX_AUX_CHANNELS 20 #define TASK_GYROPID_DESIRED_PERIOD 125 #define SCHEDULER_DELAY_LIMIT 10 #else