From da9673b9825751facf4b82074c6945428ef3277b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:23:14 +0200 Subject: [PATCH 1/9] Increase sensor Array size --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f23aa37df4..68bd96e0e0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -239,7 +239,7 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -static const char * const sensorHardwareNames[4][11] = { +static const char * const sensorHardwareNames[4][12] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, From 4ff280209511e242205da2c38ee10b8b2a649735 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 16:18:06 +0200 Subject: [PATCH 2/9] Fix CPU bug due toe reinitialising of filters --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b8b5cf11e0..6ffaf93d2f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dtermSetpointWeight = 120; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 15; + pidProfile->toleranceBand = 0; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f91875e2a..09364a8b3a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -116,11 +116,13 @@ void initFilters(const pidProfile_t *pidProfile) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + dtermBiquadLpfInitialised = true; } } } From 629f96969c48f3d48be5c6959317b66bc569f147 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 16:38:38 +0200 Subject: [PATCH 3/9] Added MPU9250 to acc lookup table. Duplication. Missing in Allowed Values printout. Duplication... --- src/main/io/serial_cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e0..3ebafa4ec6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -450,6 +450,7 @@ static const char * const lookupTableAccHardware[] = { "LSM303DLHC", "MPU6000", "MPU6500", + "MPU9250", "FAKE" }; From 685ae3aee376879e536d009de3b37c4def054cce Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:47:52 +0200 Subject: [PATCH 4/9] Limit number of targets to build on Travis. --- .travis.yml | 69 ++++++++++++++++++++++++++++------------------------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca73f8c14e..787ac12dea 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,51 +2,54 @@ env: # - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True - - TARGET=AFROMINI - - TARGET=AIORACERF3 - - TARGET=AIR32 - - TARGET=ALIENFLIGHTF1 +# - TARGET=AFROMINI +# - TARGET=AIORACERF3 +# - TARGET=AIR32 +# - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CHEBUZZF3 - - TARGET=CJMCU - - TARGET=COLIBRI - - TARGET=COLIBRI_RACE - - TARGET=DOGE - - TARGET=EUSTM32F103RC - - TARGET=F4BY - - TARGET=FURYF3 +# - TARGET=CHEBUZZF3 +# - TARGET=CJMCU +# - TARGET=COLIBRI +# - TARGET=COLIBRI_RACE +# - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 - TARGET=FURYF4 - - TARGET=IRCFUSIONF3 - - TARGET=KISSFC - - TARGET=LUX_RACE - - TARGET=MICROSCISKY - - TARGET=MOTOLAB +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=OLIMEXINO - - TARGET=OMNIBUS - - TARGET=OMNIBUSF4 - - TARGET=PIKOBLX - - TARGET=PORT103R +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R - TARGET=REVO - - TARGET=REVONANO - - TARGET=REVO_OPBL - - TARGET=RMDO - - TARGET=SINGULARITY - - TARGET=SIRINFPV +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV - TARGET=SPARKY - - TARGET=SPARKY2 - - TARGET=SPARKY_OPBL +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO - - TARGET=SPRACINGF3MINI +# - TARGET=SPRACINGF3MINI - TARGET=STM32F3DISCOVERY - - TARGET=VRRACE - - TARGET=X_RACERSPI - - TARGET=ZCOREF3 +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 +CC3D NAZE +ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY +ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From 69aecd9e926fdb372366970a3984fb8a3949eebe Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:50:22 +0200 Subject: [PATCH 5/9] Limit number of targets to build on Travis. --- .travis.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 787ac12dea..16b89723f4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,9 +47,6 @@ env: # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 -CC3D NAZE -ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY -ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From a396ba0c119c6008b985730c742b18c7f9d7c36b Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Tue, 9 Aug 2016 19:53:18 +0200 Subject: [PATCH 6/9] cleanup cli commands outputs and over - deleted printSectionBreak and his #define (not more used) - cleaned cli commads output (dump, sd_info, version) --- src/main/io/serial_cli.c | 25 +++++-------------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ebafa4ec6..1850472799 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -927,7 +927,6 @@ const clivalue_t valueTable[] = { #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) - typedef union { int32_t int_value; float float_value; @@ -940,8 +939,6 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); -#define printSectionBreak() cliPrintf((char *)sectionBreak) - #define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); @@ -1731,8 +1728,6 @@ static void printServo(uint8_t dumpMask, master_t *defaultConfig) ); } - printSectionBreak(); - // print servo directions unsigned int channel; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -2022,10 +2017,9 @@ static void cliSdInfo(char *cmdline) { ; // Nothing more detailed to print break; } - - cliPrint("\r\n"); break; } + cliPrint("\r\n"); } #endif @@ -2326,14 +2320,12 @@ static void printConfig(char *cmdline, bool doDiff) if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrint("\r\n# version\r\n"); cliVersion(NULL); - cliPrint("\r\n"); if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); } - printSectionBreak(); - cliPrint("# name\r\n"); + cliPrint("\r\n# name\r\n"); printName(dumpMask); cliPrint("\r\n# mixer\r\n"); @@ -2460,7 +2452,7 @@ static void printConfig(char *cmdline, bool doDiff) cliPrint("\r\n# adjrange\r\n"); printAdjustmentRange(dumpMask, &defaultConfig); - cliPrintf("\r\n# rxrange\r\n"); + cliPrint("\r\n# rxrange\r\n"); printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS @@ -2519,26 +2511,19 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; - changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); - - printSectionBreak(); dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); - - cliRateProfile(""); } static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values - return; + return; changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); - printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } @@ -3338,7 +3323,7 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# BetaFlight/%s %s %s / %s (%s)", + cliPrintf("# BetaFlight/%s %s %s / %s (%s)\r\n", targetName, FC_VERSION_STRING, buildDate, From 220145d788aacf612a656cb0a77445f852459503 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 22:57:54 +0200 Subject: [PATCH 7/9] Set back the max limit for rc rates --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e0..b45ece523c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -789,8 +789,8 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, From 98eb820cdbd06b629335aa5f18f02531a7745938 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:34:12 +0200 Subject: [PATCH 8/9] defaults --- src/main/config/config.c | 13 ++++++------- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6ffaf93d2f..fd74196200 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -206,7 +206,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 25; + pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -232,15 +232,15 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 50; + pidProfile->rollPitchItermIgnoreRate = 130; + pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; - pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; + pidProfile->pidAtMinThrottle = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; @@ -310,10 +310,9 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) { #ifdef BRUSHED_MOTORS escAndServoConfig->minthrottle = 1000; - escAndServoConfig->maxthrottle = 2000; #else - escAndServoConfig->minthrottle = 1150; - escAndServoConfig->maxthrottle = 1850; + escAndServoConfig->maxthrottle = 2000; + escAndServoConfig->minthrottle = 1070; #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 09364a8b3a..2e136af636 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -274,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c12404ec5a..12fa8324e8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -91,7 +91,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage - uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. + uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. // Betaflight PID controller parameters uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5947e954c1..7eedf297ae 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -842,7 +842,7 @@ const clivalue_t valueTable[] = { { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, - { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, + { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, From ea67304a1a7f683a3cba644ed68b5a20ea68e23f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:38:07 +0200 Subject: [PATCH 9/9] Fix mw.c --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index b2c569e98b..3652108743 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -536,7 +536,7 @@ void processRx(void) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); - if (currentProfile->pidProfile.zeroThrottleStabilisation) + if (currentProfile->pidProfile.pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF);