From a85715f1dd14b913ab37886752f01919ae768a46 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 2 Feb 2017 19:38:33 +0000 Subject: [PATCH 01/20] Minor efficiency improvements to fc_rc.c --- src/main/fc/fc_rc.c | 36 +++++++++++++++++++----------------- src/main/fc/fc_rc.h | 6 ------ 2 files changed, 19 insertions(+), 23 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 950054e3e3..5c8d68b43e 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -66,10 +66,10 @@ float getThrottlePIDAttenuation(void) { #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 +77,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 +102,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 +123,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; @@ -208,7 +210,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)) diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 34114b053c..94eb71b4e0 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); From 662b9552dff5d7f2f5ad5354bf4a30a5dfe41875 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sun, 5 Feb 2017 13:51:22 +0100 Subject: [PATCH 02/20] fixes issue with power changes without prior frequency change As trampConfFreq & trampConfPower was not initialised properly with the current values, this would lead to the config not being properly changed (you cannot set the frequency to 0, which is what we tried earlier). Also, a fixed number of retries has been added to make the code safer and finish after a fixed number of iteration, no matter what. --- src/main/io/vtx_tramp.c | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) 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; From dc3b89e78c67d6bd027d57733cd80648f47092f1 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 3 Feb 2017 09:51:22 +1100 Subject: [PATCH 03/20] check for PWM_rate limits --- src/main/fc/config.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d5deb21ff5..9acb623d49 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -937,8 +937,12 @@ void activateConfig(void) void validateAndFixConfig(void) { - if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ - motorConfig()->mincommand = 1000; + if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { + motorConfigMutable()->mincommand = 1000; + } + + if((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 900)) { + motorConfig()->motorPwmRate = 400; } if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { From 297f5ba2af85075e5dfb4e88cff288507b582b60 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 3 Feb 2017 14:51:25 +1100 Subject: [PATCH 04/20] Typo. Restriction should be if greater than 400. --- src/main/fc/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 9acb623d49..4b06396ad4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -941,7 +941,7 @@ void validateAndFixConfig(void) motorConfigMutable()->mincommand = 1000; } - if((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 900)) { + if((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 400)) { motorConfig()->motorPwmRate = 400; } From 9f17bae3e4cb0b5379de702a5084edede9d77751 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 6 Feb 2017 04:54:35 +1100 Subject: [PATCH 05/20] Fixed possible pid process denom exceeding allowed values --- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 14 ++++++++------ src/main/flight/pid.h | 13 +++++++------ 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e39a2f6c39..b17bb03b72 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -710,7 +710,7 @@ 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 4b06396ad4..f7c2af5956 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -937,11 +937,11 @@ void activateConfig(void) void validateAndFixConfig(void) { - if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { + if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { motorConfigMutable()->mincommand = 1000; } - if((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 400)) { + if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 400)) { motorConfig()->motorPwmRate = 400; } @@ -1093,7 +1093,7 @@ void validateAndFixGyroConfig(void) float motorUpdateRestriction; switch(motorConfig()->motorPwmProtocol) { case (PWM_TYPE_STANDARD): - motorUpdateRestriction = 0.002f; + motorUpdateRestriction = 0.0025f; break; case (PWM_TYPE_ONESHOT125): motorUpdateRestriction = 0.0005f; @@ -1113,11 +1113,13 @@ void validateAndFixGyroConfig(void) motorUpdateRestriction = 0.00003125f; } - if(pidLooptime < motorUpdateRestriction) - pidConfig()->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)) { uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); if(motorConfig()->motorPwmRate > maxEscRate) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 2c40241d34..f0f756a6a6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,12 +19,13 @@ #include -#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 From a3c61a5114309911313aeefb777f524a0fdf4c12 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 6 Feb 2017 16:09:54 +0100 Subject: [PATCH 06/20] Bump patch version --- src/main/build/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index c34f3050a7..3394464af4 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -18,7 +18,7 @@ #define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 3 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From 63b7e3eaa2c670053afdf22ba0d0f02e6c9388c2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 6 Feb 2017 16:24:52 +0100 Subject: [PATCH 07/20] Fix merge // Add target config bff3 --- src/main/fc/config.c | 4 +-- src/main/target/BETAFLIGHTF3/config.c | 44 +++++++++++++++++++++++++++ src/main/target/BETAFLIGHTF3/target.h | 1 + 3 files changed, 47 insertions(+), 2 deletions(-) create mode 100755 src/main/target/BETAFLIGHTF3/config.c diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f7c2af5956..f8a103c5f8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -938,7 +938,7 @@ void activateConfig(void) void validateAndFixConfig(void) { if ((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { - motorConfigMutable()->mincommand = 1000; + motorConfig()->mincommand = 1000; } if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 400)) { @@ -1115,7 +1115,7 @@ void validateAndFixGyroConfig(void) 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); + pidConfig()->pid_process_denom = MIN(pidConfig()->pid_process_denom, maxPidProcessDenom); } // Prevent overriding the max rate of motors diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c new file mode 100755 index 0000000000..b65b136e04 --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -0,0 +1,44 @@ +/* + * 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 + +#include "common/utils.h" + +#include "drivers/io.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +void targetConfiguration(master_t *config) +{ + UNUSED(config); + + batteryConfig->currentMeterScale = 235; +} + diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index a81ea68ea0..c327da44e5 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 From 428f39341cbe94fe46963e8a4c0cef566fbc4ad2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 6 Feb 2017 23:20:01 +0100 Subject: [PATCH 08/20] Remove unused var --- src/main/fc/rc_controls.c | 1 - src/main/fc/rc_controls.h | 1 - 2 files changed, 2 deletions(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8a64ed70c7..29137dd160 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -65,7 +65,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 e416b682b1..e9c88f874b 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -159,7 +159,6 @@ typedef struct controlRateConfig_s { } controlRateConfig_t; 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. From 80c0e32660558781d33e9238253653ccfa3365a1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 6 Feb 2017 23:48:01 +0100 Subject: [PATCH 09/20] Restore multiwii throttle curve --- src/main/fc/config.c | 3 +++ src/main/fc/fc_msp.c | 2 ++ src/main/fc/fc_rc.c | 34 +++++++++++++++++++++++++++------- src/main/fc/fc_rc.h | 1 + src/main/fc/rc_controls.c | 2 ++ 5 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f8a103c5f8..31ede0bf82 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -53,6 +53,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/fc_rc.h" #include "fc/runtime_config.h" #include "sensors/sensors.h" @@ -883,6 +884,8 @@ static void resetConf(void) void activateConfig(void) { + generateThrottleCurve(); + resetAdjustmentStates(); useRcControlsConfig( diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 775b3f190e..6e7adf2027 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -52,6 +52,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" @@ -1399,6 +1400,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 5c8d68b43e..8344d65d40 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -63,6 +63,32 @@ 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 @@ -269,13 +295,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 94eb71b4e0..7c2f378645 100755 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -23,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_controls.c b/src/main/fc/rc_controls.c index 29137dd160..0f8a79b01e 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -37,6 +37,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" @@ -522,6 +523,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: From 7f8ed026bf86e3e0f3474aba11c04999a0f3ea61 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 00:05:38 +0100 Subject: [PATCH 10/20] Optimise disabled anti gravity gain // disable by default --- src/main/fc/config.c | 2 +- src/main/fc/fc_rc.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 31ede0bf82..b81e3d37b1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -183,7 +183,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 350; - pidProfile->itermAcceleratorGain = 2.0f; + pidProfile->itermAcceleratorGain = 1.0f; pidProfile->itermAcceleratorRateLimit = 80; } diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 8344d65d40..9787a03877 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -181,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) { From ce106621521705821706b1e94f020d838341993f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 00:31:16 +0100 Subject: [PATCH 11/20] Cleanup eeprom verification restrictions --- src/main/fc/config.c | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b81e3d37b1..3c25b29078 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -100,11 +100,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; @@ -944,8 +940,8 @@ void validateAndFixConfig(void) motorConfig()->mincommand = 1000; } - if ((motorConfig()->motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->motorPwmRate > 400)) { - motorConfig()->motorPwmRate = 400; + 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))) { @@ -1096,7 +1092,7 @@ void validateAndFixGyroConfig(void) float motorUpdateRestriction; switch(motorConfig()->motorPwmProtocol) { case (PWM_TYPE_STANDARD): - motorUpdateRestriction = 0.0025f; + motorUpdateRestriction = 0.002f; break; case (PWM_TYPE_ONESHOT125): motorUpdateRestriction = 0.0005f; From 6d6853b2674bef11c1a775ae6c4b60f93be02dce Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 00:53:01 +0100 Subject: [PATCH 12/20] Move setpoint transition point --- src/main/config/config_eeprom.h | 2 +- src/main/fc/config.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 645689ffd0..9ba398e4b7 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,7 +17,7 @@ #pragma once -#define EEPROM_CONF_VERSION 154 +#define EEPROM_CONF_VERSION 155 void initEEPROM(void); void writeEEPROM(); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3c25b29078..20b43812b1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -174,7 +174,7 @@ 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; From ea2c8a6fdeb3f497564a31cfcbe067003daf867a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 00:55:17 +0100 Subject: [PATCH 13/20] Fix for default aux channel being out of range --- src/main/fc/cli.c | 2 +- src/main/target/common.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b17bb03b72..9e703c2fc9 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -507,7 +507,7 @@ 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, 20 } }, { "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 } }, diff --git a/src/main/target/common.h b/src/main/target/common.h index a9270aa8af..38e562c186 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -74,7 +74,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 From 377893e173e3bac5b02d85d0c36b03fd89143d09 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 2 Feb 2017 21:17:45 +0100 Subject: [PATCH 14/20] Register SDCard DMA in resource list --- src/main/drivers/resource.c | 3 ++- src/main/drivers/resource.h | 3 ++- src/main/drivers/sdcard.c | 4 ++++ 3 files changed, 8 insertions(+), 2 deletions(-) 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; From 2ab89a40e0af31895a1363d9114cced7c7cc61f7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 01:01:49 +0100 Subject: [PATCH 15/20] Enable Serial SBUS by default on BFF3 --- src/main/target/BETAFLIGHTF3/target.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index c327da44e5..cd94249f4d 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -123,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 From 17bcb0c09202dd6fef5c34029ed7c742c7dfe0c7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 01:11:08 +0100 Subject: [PATCH 16/20] Correct Standard PWM rate --- src/main/fc/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 20b43812b1..c0734610c6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1092,7 +1092,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; From 1ec8adbb5c4dff9166f83e875706ff59c958bdf2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 01:12:27 +0100 Subject: [PATCH 17/20] MAX_AUX_CHANNELS for cli max constrain --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9e703c2fc9..88ab90aa4e 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -507,7 +507,7 @@ 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, 20 } }, + { "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 } }, From 2a77107376557fa9d2c3992230c80cc029853afd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 01:49:14 +0100 Subject: [PATCH 18/20] Prevent resetting motorPwmRate twice for PWM_TYPE_STANDARD --- src/main/fc/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c0734610c6..888323cbb5 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1118,7 +1118,7 @@ void validateAndFixGyroConfig(void) } // 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) From 89527df273741186d31a0d2c973687b9ae46077b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 10:26:21 +0100 Subject: [PATCH 19/20] Simplify anti gravity --- src/main/fc/cli.c | 1 - src/main/fc/config.c | 1 - src/main/flight/pid.c | 10 ++-------- src/main/flight/pid.h | 1 - 4 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 88ab90aa4e..9826e3cf19 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -702,7 +702,6 @@ 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 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 888323cbb5..946ab49c40 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -180,7 +180,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 350; pidProfile->itermAcceleratorGain = 1.0f; - pidProfile->itermAcceleratorRateLimit = 80; } void resetProfile(profile_t *profile) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7748679e4b..3ee6ff0963 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -152,7 +152,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++) { @@ -169,7 +169,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) { @@ -255,12 +254,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 f0f756a6a6..9c045245c6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -79,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 From b7605f1d8f2cfe95c6c26f5cb8caef16fb8a816d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 14:05:36 +0100 Subject: [PATCH 20/20] Fix merge artifacts --- src/main/fc/rc_adjustments.c | 3 +- src/main/fc/rc_curves.c | 60 --------------------------- src/main/fc/rc_curves.h | 25 ----------- src/main/target/BETAFLIGHTF3/config.c | 18 +------- 4 files changed, 4 insertions(+), 102 deletions(-) delete mode 100644 src/main/fc/rc_curves.c delete mode 100644 src/main/fc/rc_curves.h 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_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/fc/rc_curves.h b/src/main/fc/rc_curves.h deleted file mode 100644 index c13851969e..0000000000 --- a/src/main/fc/rc_curves.h +++ /dev/null @@ -1,25 +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 . - */ - -#pragma once - -struct controlRateConfig_s; -struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); - -int16_t rcLookupThrottle(int32_t tmp); - diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index b65b136e04..e7b417c39e 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -20,25 +20,11 @@ #include -#include "common/utils.h" - -#include "drivers/io.h" - -#include "fc/rc_controls.h" - -#include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" void targetConfiguration(master_t *config) { - UNUSED(config); - - batteryConfig->currentMeterScale = 235; + config->batteryConfig.currentMeterScale = 235; }