From b20dc77a7428aeba237d215789160a065bdcbf24 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 26 Feb 2015 20:40:28 +0100 Subject: [PATCH 01/17] Harakiri PID controller make hardcoded parameters configurable --- src/main/config/config.c | 3 +++ src/main/flight/pid.c | 8 ++++++-- src/main/flight/pid.h | 2 ++ src/main/io/serial_cli.c | 3 +++ 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5c324f4206..83be966400 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -187,6 +187,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; pidProfile->H_sensitivity = 75; + + pidProfile->pid5_oldyw = 0; + pidProfile->pid5_maincuthz = 12; } #ifdef GPS diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 899015ce44..8fa3e424db 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -577,7 +577,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) uint8_t axis; float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; - MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz) + if (pidProfile->dterm_cut_hz) { + MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // dterm_cut_hz (default 0, Range 1-50Hz) + } else { + MainDptCut = RCconstPI / 12.0f; // default is 12Hz to maintain initial behavior of PID5 + } FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element @@ -589,7 +593,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) for (axis = 0; axis < 2; axis++) { int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight - rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers + rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3ec0004e96..6dc552726f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -61,7 +61,9 @@ typedef struct pidProfile_s { float A_level; float H_level; uint8_t H_sensitivity; + uint16_t yaw_p_limit; // set P term limit (fixed value was 300) + uint8_t pid5_oldyw; // Old yaw behavior for PID5 uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames uint8_t gyro_cut_hz; // Used for soft gyro filtering diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eb083821d4..1f3499d428 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -514,6 +514,9 @@ const clivalue_t valueTable[] = { { "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 }, { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, + { "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 }, + { "pid5_maincuthz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 }, + #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, From 43f5792a6167d108191e322282d0d3944cc1a770 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 27 Feb 2015 07:38:51 +0100 Subject: [PATCH 02/17] G-Tune port from Harakiri Enabled for NAZE, ALIENWWIIF1 and ALIENWIIF3 targets Implement G-Tune for all PID controllers The G-Tune tuning results will be save if G-Tune mode will be disabled during copter is disarmed. Update PID controller and G-Tune documentation --- Makefile | 1 + docs/Gtune.md | 46 ++++++++ docs/PID tuning.md | 6 +- src/main/config/config.c | 10 ++ src/main/config/runtime_config.h | 1 + src/main/flight/gtune.c | 164 ++++++++++++++++++++++++++++ src/main/flight/gtune.h | 21 ++++ src/main/flight/pid.c | 69 ++++++++++++ src/main/flight/pid.h | 9 +- src/main/io/rc_controls.h | 1 + src/main/io/serial_cli.c | 10 ++ src/main/io/serial_msp.c | 6 + src/main/mw.c | 29 +++++ src/main/target/ALIENWIIF3/target.h | 1 + src/main/target/NAZE/target.h | 1 + 15 files changed, 371 insertions(+), 4 deletions(-) create mode 100644 docs/Gtune.md create mode 100644 src/main/flight/gtune.c create mode 100644 src/main/flight/gtune.h diff --git a/Makefile b/Makefile index 00de825b5c..84df74f392 100755 --- a/Makefile +++ b/Makefile @@ -261,6 +261,7 @@ COMMON_SRC = build_config.c \ $(DEVICE_STDPERIPH_SRC) HIGHEND_SRC = flight/autotune.c \ + flight/gtune.c \ flight/navigation.c \ flight/gps_conversion.c \ common/colorconversion.c \ diff --git a/docs/Gtune.md b/docs/Gtune.md new file mode 100644 index 0000000000..46ac2c8c0c --- /dev/null +++ b/docs/Gtune.md @@ -0,0 +1,46 @@ +# G-Tune instructions. + +The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com) +http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html +http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2 +http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190 + +The G-Tune functionality for Cleanflight is ported from the Harakiri firmware. + +- Safety preamble: Use at your own risk - + +The implementation you have here is quiet different and just for adjusting the P values of ROLL/PITCH/YAW in Acro mode. +When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch). +It will start tuning the wanted / possible axes (see below) in a predefined range (see below). +After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work). +The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch +and yaw are tuned. If you stop rolling G-Tune will wait ca. 400ms to let the axis settle and then start tuning that axis again. All axes are treated independently. +The easiest way to tune all axes at once is to do some air-jumps with the copter in Acro (RC centered and G-Tune activated... of course..). +You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below). +Yaw tune is disabled in any copter with less than 4 motors (like tricopters). +G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...) +You will see the results in the GUI - the the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.) +TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you. + +## Parameters and their function: + +gtune_loP_rll = 20 [10..200] Lower limit of ROLL P during G-Tune. Note "20" means "2.0" in the GUI. +gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI. +gtune_loP_yw = 20 [10..200] Lower limit of YAW P during G-Tune. Note "20" means "2.0" in the GUI. +gtune_hiP_rll = 70 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. +gtune_hiP_ptch = 70 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. +gtune_hiP_yw = 70 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. +gtune_pwr = 0 [0..10] Strength of adjustment + +So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups. +If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P", +you are in maybe sluggish initial control. +If you want to exclude one axis from the tuning you must set gtune_hiP_XXX to zero. Let's say you want to disable yaw tuning write in CLI +"set gtune_hiP_yw = 0". Note: The MultiWii Wiki advises you to trim the yaw axis on your transmitter. If you have done so (yaw not neutral on your RC) +yaw tuning will be disabled. + +Setting the strength of tuning: +gtune_pwr [0..10] Strength of adjustment. +My small copter works fine with 0 and doesn't like a value of "3". My big copter likes "gtune_pwr = 5". It shifts the tuning to higher values and if too high can +diminish the wobble blocking! So start with 0 (default). If you feel your resulting P is always too low for you increase gtune_pwr. You will see it getting a little shaky +if value too high. diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 8c41f4ce82..972584c5e8 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -150,10 +150,10 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. There are two additional settings which are configurable via the CLI in Harakiri: - OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. - MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) + set pid5_maincuthz = 12 [1-50Hz] Cut Off Frequency for D term of main Pid controller + set pid5_oldyw = 0 [0/1] 0 = multiwii 2.3 yaw (default), 1 = older yaw The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. diff --git a/src/main/config/config.c b/src/main/config/config.c index 83be966400..8dc232f34f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -190,6 +190,16 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pid5_oldyw = 0; pidProfile->pid5_maincuthz = 12; + +#ifdef GTUNE + pidProfile->gtune_lolimP[ROLL] = 20; // [10..200] Lower limit of ROLL P during G tune. + pidProfile->gtune_lolimP[PITCH] = 20; // [10..200] Lower limit of PITCH P during G tune. + pidProfile->gtune_lolimP[YAW] = 20; // [10..200] Lower limit of YAW P during G tune. + pidProfile->gtune_hilimP[ROLL] = 70; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_hilimP[PITCH] = 70; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_hilimP[YAW] = 70; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment +#endif } #ifdef GPS diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index c5a1bff6ac..14e8b0fc34 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -42,6 +42,7 @@ typedef enum { PASSTHRU_MODE = (1 << 8), SONAR_MODE = (1 << 9), FAILSAFE_MODE = (1 << 10), + GTUNE_MODE = (1 << 11), } flightModeFlags_e; extern uint16_t flightModeFlags; diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c new file mode 100644 index 0000000000..b0011d2e50 --- /dev/null +++ b/src/main/flight/gtune.c @@ -0,0 +1,164 @@ +/* + * 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 + +#include "platform.h" + +#ifdef GTUNE + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" + +#include "flight/pid.h" +#include "flight/imu.h" + +#include "config/config.h" +#include "blackbox/blackbox.h" + +#include "io/rc_controls.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; + +/* + **************************************************************************** + *** G_Tune *** + **************************************************************************** + G_Tune Mode + This is the multiwii implementation of ZERO-PID Algorithm + http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html + The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com) + + You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution. + */ + +/* + version 1.0.0: MIN & Maxis & Tuned Band + version 1.0.1: + a. error is gyro reading not rc - gyro. + b. OldError = Error no averaging. + c. No Min Maxis BOUNDRY + version 1.0.2: + a. no boundaries + b. I - Factor tune. + c. time_skip + + Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore. + Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well. + See also: + http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2 + http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190 + Gyrosetting 2000DPS + GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s + + pidProfile->gtune_lolimP[ROLL] = 20; [10..200] Lower limit of ROLL P during G tune. + pidProfile->gtune_lolimP[PITCH] = 20; [10..200] Lower limit of PITCH P during G tune. + pidProfile->gtune_lolimP[YAW] = 20; [10..200] Lower limit of YAW P during G tune. + pidProfile->gtune_hilimP[ROLL] = 70; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_hilimP[PITCH] = 70; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_hilimP[YAW] = 70; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment +*/ + +static pidProfile_t *pidProfile; +static int8_t time_skip[3]; +static int16_t OldError[3], result_P64[3]; +static int32_t AvgGyro[3]; +static bool floatPID; + +void init_Gtune(pidProfile_t *pidProfileToTune) +{ + uint8_t i; + + pidProfile = pidProfileToTune; + if (pidProfile->pidController == 2) floatPID = true; // LuxFloat is using float values for PID settings + else floatPID = false; + for (i = 0; i < 3; i++) { + if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning + (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0; // Disable Yawtuning for everything below a quadcopter + if (floatPID) { + if(pidProfile->P_f[i] < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = pidProfile->gtune_lolimP[i]; + result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. + } else { + if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i]; + result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P. + } + OldError[i] = 0; + time_skip[i] = -125; + } +} + +void calculate_Gtune(uint8_t axis) +{ + int16_t error, diff_G, threshP; + + if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block Tuning on stickinput. Always allow Gtune on YAW, Roll & Pitch only in acromode + OldError[axis] = 0; + time_skip[axis] = -125; // Some settletime after stick center. (125 + 16)* 3ms clycle = 423ms (ca.) + } else { + if (!time_skip[axis]) AvgGyro[axis] = 0; + time_skip[axis]++; + if (time_skip[axis] > 0) { + if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average + else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128); // Chop some jitter and average + } + if (time_skip[axis] == 16) { // ca 48 ms + AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata + time_skip[axis] = 0; + if (axis == FD_YAW) { + threshP = 20; + error = -AvgGyro[axis]; + } else { + threshP = 10; + error = AvgGyro[axis]; + } + if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so + diff_G = ABS(error) - ABS(OldError[axis]); + if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) { + if (diff_G > threshP) result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side. + else { + if (diff_G < -threshP) { + if (axis == FD_YAW) result_P64[axis] -= 64 + pidProfile->gtune_pwr; + else result_P64[axis] -= 32; + } + } + } else { + if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW + } + result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6); + if (floatPID) pidProfile->P_f[axis] = (float)(result_P64[axis] >> 6); // new P value for float PID + else pidProfile->P8[axis] = result_P64[axis] >> 6; // new P value + } + OldError[axis] = error; + } + } +} + +#endif + diff --git a/src/main/flight/gtune.h b/src/main/flight/gtune.h new file mode 100644 index 0000000000..28f39fa926 --- /dev/null +++ b/src/main/flight/gtune.h @@ -0,0 +1,21 @@ +/* + * 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 + +void init_Gtune(pidProfile_t *pidProfileToTune); +void calculate_Gtune(uint8_t axis); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8fa3e424db..a9fc9ff97d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -42,6 +42,7 @@ #include "flight/imu.h" #include "flight/navigation.h" #include "flight/autotune.h" +#include "flight/gtune.h" #include "flight/filter.h" #include "config/runtime_config.h" @@ -222,6 +223,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; @@ -317,6 +324,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; @@ -408,6 +421,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[axis] = PTerm + ITerm - DTerm; +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; @@ -437,6 +456,17 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[FD_YAW] = PTerm + ITerm; + if (motorCount >= 4) { // prevent "yaw jump" during yaw correction + int16_t limit = ABS(rcCommand[FD_YAW]) + 100; + axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); + } + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(FD_YAW); + } +#endif + #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; axisPID_I[FD_YAW] = ITerm; @@ -530,6 +560,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; @@ -558,6 +594,16 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con axisPID[FD_YAW] = PTerm + ITerm; + if (motorCount >= 4) { // prevent "yaw jump" during yaw correction + int16_t limit = ABS(rcCommand[FD_YAW]) + 100; + axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); + } + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(FD_YAW); + } +#endif #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; @@ -650,6 +696,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result. +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; @@ -697,6 +749,17 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. + if (motorCount >= 4) { // prevent "yaw jump" during yaw correction + int16_t limit = ABS(rcCommand[FD_YAW]) + 100; + axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); + } + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(FD_YAW); + } +#endif + #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; axisPID_I[FD_YAW] = ITerm; @@ -823,6 +886,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6dc552726f..21a6e048ff 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -63,10 +63,17 @@ typedef struct pidProfile_s { uint8_t H_sensitivity; uint16_t yaw_p_limit; // set P term limit (fixed value was 300) - uint8_t pid5_oldyw; // Old yaw behavior for PID5 uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames uint8_t gyro_cut_hz; // Used for soft gyro filtering + + uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw + +#ifdef GTUNE + uint8_t gtune_lolimP[3]; // [10..200] Lower limit of P during G tune + uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis. + int8_t gtune_pwr; // [0..10] Strength of adjustment +#endif } pidProfile_t; #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8be2586484..6d5f28be2c 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -48,6 +48,7 @@ typedef enum { BOXSERVO3, BOXBLACKBOX, BOXFAILSAFE, + BOXGTUNE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1f3499d428..7ee8e43146 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -517,6 +517,16 @@ const clivalue_t valueTable[] = { { "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 }, { "pid5_maincuthz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 }, +#ifdef GTUNE + { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 }, + { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 }, + { "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], 10, 200 }, + { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], 0, 200 }, + { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], 0, 200 }, + { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], 0, 200 }, + { "gtune_pwr", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 }, +#endif + #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f383acbf82..c662b4c3d1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -347,6 +347,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXSERVO3, "SERVO3;", 25 }, { BOXBLACKBOX, "BLACKBOX;", 26 }, { BOXFAILSAFE, "FAILSAFE;", 27 }, + { BOXGTUNE, "GTUNE;", 28 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -703,6 +704,10 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; } +#ifdef GTUNE + activeBoxIds[activeBoxIdCount++] = BOXGTUNE; +#endif + memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } @@ -822,6 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | diff --git a/src/main/mw.c b/src/main/mw.c index 4174d0e173..7553f7d698 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -71,6 +71,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/autotune.h" +#include "flight/gtune.h" #include "flight/navigation.h" #include "flight/filter.h" @@ -160,6 +161,30 @@ void updateAutotuneState(void) } #endif +#ifdef GTUNE + +void updateGtuneState(void) +{ + static bool GTuneWasUsed = false; + + if (IS_RC_MODE_ACTIVE(BOXGTUNE)) { + if (!FLIGHT_MODE(GTUNE_MODE)) { + ENABLE_FLIGHT_MODE(GTUNE_MODE); + init_Gtune(¤tProfile->pidProfile); + GTuneWasUsed = true; + } + } else { + if (FLIGHT_MODE(GTUNE_MODE)) { + DISABLE_FLIGHT_MODE(GTUNE_MODE); + if (!ARMING_FLAG(ARMED) && GTuneWasUsed) { + saveConfigAndNotify(); + GTuneWasUsed = false; + } + } + } +} +#endif + bool isCalibrating() { #ifdef BARO @@ -806,6 +831,10 @@ void loop(void) } #endif +#ifdef GTUNE + updateGtuneState(); +#endif + #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h index 70b3367ee2..f9291e3674 100644 --- a/src/main/target/ALIENWIIF3/target.h +++ b/src/main/target/ALIENWIIF3/target.h @@ -115,6 +115,7 @@ //#define GPS //#define DISPLAY #define AUTOTUNE +#define GTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 8d2c665754..bba37c7e98 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -175,6 +175,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define GTUNE #define USE_SERVOS #define USE_CLI From cb5f81ca988078635c07f4052b5fa79fd991b5cb Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 1 Mar 2015 01:01:11 +0100 Subject: [PATCH 03/17] G-Tune documentation update --- docs/Gtune.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/Gtune.md b/docs/Gtune.md index 46ac2c8c0c..3fff96aa43 100644 --- a/docs/Gtune.md +++ b/docs/Gtune.md @@ -24,13 +24,13 @@ TPA and G-Tune: It is not tested and will most likely not result into something ## Parameters and their function: -gtune_loP_rll = 20 [10..200] Lower limit of ROLL P during G-Tune. Note "20" means "2.0" in the GUI. -gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI. -gtune_loP_yw = 20 [10..200] Lower limit of YAW P during G-Tune. Note "20" means "2.0" in the GUI. -gtune_hiP_rll = 70 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. -gtune_hiP_ptch = 70 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. -gtune_hiP_yw = 70 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. -gtune_pwr = 0 [0..10] Strength of adjustment + gtune_loP_rll = 20 [10..200] Lower limit of ROLL P during G-Tune. Note "20" means "2.0" in the GUI. + gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI. + gtune_loP_yw = 20 [10..200] Lower limit of YAW P during G-Tune. Note "20" means "2.0" in the GUI. + gtune_hiP_rll = 70 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. + gtune_hiP_ptch = 70 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. + gtune_hiP_yw = 70 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. + gtune_pwr = 0 [0..10] Strength of adjustment So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups. If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P", From daceb2db9a61d0df3fd86ec17490182107831fd6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 2 Mar 2015 00:12:01 +0100 Subject: [PATCH 04/17] Fix G-Tune for LuxFloat PID controller G-Tune documentation fix --- docs/Gtune.md | 2 +- src/main/flight/gtune.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Gtune.md b/docs/Gtune.md index 3fff96aa43..a581003255 100644 --- a/docs/Gtune.md +++ b/docs/Gtune.md @@ -19,7 +19,7 @@ The easiest way to tune all axes at once is to do some air-jumps with the copter You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below). Yaw tune is disabled in any copter with less than 4 motors (like tricopters). G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...) -You will see the results in the GUI - the the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.) +You will see the results in the GUI - the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.) TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you. ## Parameters and their function: diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index b0011d2e50..b3236a3ef0 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -103,7 +103,7 @@ void init_Gtune(pidProfile_t *pidProfileToTune) if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0; // Disable Yawtuning for everything below a quadcopter if (floatPID) { - if(pidProfile->P_f[i] < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = pidProfile->gtune_lolimP[i]; + if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10); result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. } else { if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i]; @@ -152,7 +152,7 @@ void calculate_Gtune(uint8_t axis) if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW } result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6); - if (floatPID) pidProfile->P_f[axis] = (float)(result_P64[axis] >> 6); // new P value for float PID + if (floatPID) pidProfile->P_f[axis] = (float)((result_P64[axis] >> 6) / 10); // new P value for float PID else pidProfile->P8[axis] = result_P64[axis] >> 6; // new P value } OldError[axis] = error; From 6c022455c510b9d85731990203324d583d4a5f1f Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 2 Mar 2015 13:31:37 +0100 Subject: [PATCH 05/17] Add BlackBox recording for G-Tune --- src/main/blackbox/blackbox.c | 4 ++++ src/main/blackbox/blackbox_fielddefs.h | 13 +++++++++++++ src/main/flight/gtune.c | 19 ++++++++++++++++--- 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2f7ac51747..fb993d2464 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1162,6 +1162,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWrite(data->inflightAdjustment.adjustmentFunction); blackboxWriteSignedVB(data->inflightAdjustment.newValue); } + case FLIGHT_LOG_EVENT_GTUNE_RESULT: + blackboxWrite(data->gtuneCycleResult.gtuneAxis); + blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG); + blackboxWrite(data->gtuneCycleResult.gtuneNewP); break; case FLIGHT_LOG_EVENT_LOGGING_RESUME: blackboxWriteUnsignedVB(data->loggingResume.logIteration); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 72ffa81481..f74e0bb526 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -108,6 +108,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, + FLIGHT_LOG_EVENT_GTUNE_RESULT = 20, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -148,10 +149,18 @@ typedef struct flightLogEvent_inflightAdjustment_t { float newFloatValue; } flightLogEvent_inflightAdjustment_t; +<<<<<<< Upstream, based on origin/master typedef struct flightLogEvent_loggingResume_t { uint32_t logIteration; uint32_t currentTime; } flightLogEvent_loggingResume_t; +======= +typedef struct flightLogEvent_gtuneCycleResult_t { + uint8_t gtuneAxis; + uint32_t gtuneGyroAVG; + uint8_t gtuneNewP; +} flightLogEvent_gtuneCycleResult_t; +>>>>>>> 6f60c52 Add BlackBox recording for G-Tune typedef union flightLogEventData_t { @@ -160,7 +169,11 @@ typedef union flightLogEventData_t flightLogEvent_autotuneCycleResult_t autotuneCycleResult; flightLogEvent_autotuneTargets_t autotuneTargets; flightLogEvent_inflightAdjustment_t inflightAdjustment; +<<<<<<< Upstream, based on origin/master flightLogEvent_loggingResume_t loggingResume; +======= + flightLogEvent_gtuneCycleResult_t gtuneCycleResult; +>>>>>>> 6f60c52 Add BlackBox recording for G-Tune } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index b3236a3ef0..d228c4aa86 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -151,9 +151,22 @@ void calculate_Gtune(uint8_t axis) } else { if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW } - result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6); - if (floatPID) pidProfile->P_f[axis] = (float)((result_P64[axis] >> 6) / 10); // new P value for float PID - else pidProfile->P8[axis] = result_P64[axis] >> 6; // new P value + int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]); + +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_gtuneCycleResult_t eventData; + + eventData.gtuneAxis = axis; + eventData.gtuneGyroAVG = AvgGyro[axis]; + eventData.gtuneNewP = newP; + + blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); + } +#endif + + if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID + else pidProfile->P8[axis] = newP; // new P value } OldError[axis] = error; } From e7e297ad5377aaf90a5d2244978b3caa57819d0b Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 3 Mar 2015 08:26:37 +0100 Subject: [PATCH 06/17] Modified behavior of G-Tune switch and storing the tuned P values G-Tune documentation update G-Tune will only activated and deactivated when armed. G-Tune should deactivated while the copter is airborne. Tuned P values will only be stored when G-Tune is enabled while disarmed and G-Tune was used before. --- docs/Gtune.md | 4 ++-- src/main/flight/pid.c | 2 +- src/main/mw.c | 14 +++++++------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/Gtune.md b/docs/Gtune.md index a581003255..6851b11929 100644 --- a/docs/Gtune.md +++ b/docs/Gtune.md @@ -10,7 +10,7 @@ The G-Tune functionality for Cleanflight is ported from the Harakiri firmware. - Safety preamble: Use at your own risk - The implementation you have here is quiet different and just for adjusting the P values of ROLL/PITCH/YAW in Acro mode. -When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch). +When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch) while the copter is armed. It will start tuning the wanted / possible axes (see below) in a predefined range (see below). After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work). The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch @@ -19,7 +19,7 @@ The easiest way to tune all axes at once is to do some air-jumps with the copter You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below). Yaw tune is disabled in any copter with less than 4 motors (like tricopters). G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...) -You will see the results in the GUI - the tuning results will only be saved if you disable G-Tune mode while the copter is disarmed or you save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.) +You will see the results in the GUI - the tuning results will only be saved if you enable G-Tune mode while the copter is disarmed and G-Tune was used before when armed. You also can save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.) TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you. ## Parameters and their function: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a9fc9ff97d..bb5026e716 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -712,7 +712,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter Mwii3msTimescale /= 3000.0f; - if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now + if (pidProfile->pid5_oldyw) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f); PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; diff --git a/src/main/mw.c b/src/main/mw.c index 7553f7d698..e540a849c8 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -168,18 +168,18 @@ void updateGtuneState(void) static bool GTuneWasUsed = false; if (IS_RC_MODE_ACTIVE(BOXGTUNE)) { - if (!FLIGHT_MODE(GTUNE_MODE)) { - ENABLE_FLIGHT_MODE(GTUNE_MODE); + if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + ENABLE_FLIGHT_MODE(GTUNE_MODE); init_Gtune(¤tProfile->pidProfile); GTuneWasUsed = true; } + if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) { + saveConfigAndNotify(); + GTuneWasUsed = false; + } } else { - if (FLIGHT_MODE(GTUNE_MODE)) { + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { DISABLE_FLIGHT_MODE(GTUNE_MODE); - if (!ARMING_FLAG(ARMED) && GTuneWasUsed) { - saveConfigAndNotify(); - GTuneWasUsed = false; - } } } } From fe2f2f30533712a92a2cd9e036a831ee65bdcc6c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 12 Apr 2015 04:46:27 +0200 Subject: [PATCH 07/17] Fix some BlackBox isues, remove redundant code from PID controllers. --- src/main/blackbox/blackbox.c | 4 ++-- src/main/blackbox/blackbox_fielddefs.h | 4 ++-- src/main/flight/gtune.c | 3 ++- src/main/flight/pid.c | 15 --------------- 4 files changed, 6 insertions(+), 20 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fb993d2464..c98390f1c5 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1164,8 +1164,8 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) } case FLIGHT_LOG_EVENT_GTUNE_RESULT: blackboxWrite(data->gtuneCycleResult.gtuneAxis); - blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG); - blackboxWrite(data->gtuneCycleResult.gtuneNewP); + blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG); + blackboxWriteS16(data->gtuneCycleResult.gtuneNewP); break; case FLIGHT_LOG_EVENT_LOGGING_RESUME: blackboxWriteUnsignedVB(data->loggingResume.logIteration); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index f74e0bb526..fcd087584d 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -157,8 +157,8 @@ typedef struct flightLogEvent_loggingResume_t { ======= typedef struct flightLogEvent_gtuneCycleResult_t { uint8_t gtuneAxis; - uint32_t gtuneGyroAVG; - uint8_t gtuneNewP; + int32_t gtuneGyroAVG; + int16_t gtuneNewP; } flightLogEvent_gtuneCycleResult_t; >>>>>>> 6f60c52 Add BlackBox recording for G-Tune diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index d228c4aa86..bb3af9b3d0 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -159,7 +159,8 @@ void calculate_Gtune(uint8_t axis) eventData.gtuneAxis = axis; eventData.gtuneGyroAVG = AvgGyro[axis]; - eventData.gtuneNewP = newP; + if (floatPID) eventData.gtuneNewP = newP / 10; + else eventData.gtuneNewP = newP; blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bb5026e716..b7f97c85ee 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -456,11 +456,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control axisPID[FD_YAW] = PTerm + ITerm; - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW); @@ -594,11 +589,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con axisPID[FD_YAW] = PTerm + ITerm; - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW); @@ -749,11 +739,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. - if (motorCount >= 4) { // prevent "yaw jump" during yaw correction - int16_t limit = ABS(rcCommand[FD_YAW]) + 100; - axisPID[FD_YAW] = (float)constrain((int32_t)axisPID[FD_YAW], -limit, +limit); - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(FD_YAW); From ef5887856d02ee2a1e49f3ce7af522e2b6ee5d64 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 12 Apr 2015 21:56:11 +0200 Subject: [PATCH 08/17] Enable G-Tune on Sparky Fix code style --- src/main/flight/gtune.c | 2 +- src/main/target/SPARKY/target.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index bb3af9b3d0..63c68afdb3 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -155,7 +155,7 @@ void calculate_Gtune(uint8_t axis) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_gtuneCycleResult_t eventData; + flightLogEvent_gtuneCycleResult_t eventData; eventData.gtuneAxis = axis; eventData.gtuneGyroAVG = AvgGyro[axis]; diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 1c835d2b02..55e7febfc3 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -108,6 +108,7 @@ #define CURRENT_METER_ADC_CHANNEL ADC_Channel_4 #define AUTOTUNE +#define GTUNE #define BLACKBOX #define TELEMETRY #define SERIAL_RX From 53531224be0c6bb9972ff3297968eab22a04d850 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 4 May 2015 14:06:09 +0200 Subject: [PATCH 09/17] Make G-Tune more configurable - add two new CLI paramaters "gtune_settle_time" and "gtune_average_cycles" - the settle time is not depending on looptime anymore - updated default setting to cover e wider range of copters - remove lower limit for P value for CLI (Zero P is now posible, but schould be used with care) - Documentation updates --- docs/Gtune.md | 23 ++++++++++++----------- src/main/config/config.c | 14 ++++++++------ src/main/flight/gtune.c | 40 +++++++++++++++++++++++++--------------- src/main/flight/pid.h | 6 ++++-- src/main/io/serial_cli.c | 4 +++- 5 files changed, 52 insertions(+), 35 deletions(-) diff --git a/docs/Gtune.md b/docs/Gtune.md index 6851b11929..d51d7adfd6 100644 --- a/docs/Gtune.md +++ b/docs/Gtune.md @@ -14,7 +14,7 @@ When flying in Acro mode (yaw tune in other modes possible as well - see below) It will start tuning the wanted / possible axes (see below) in a predefined range (see below). After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work). The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch -and yaw are tuned. If you stop rolling G-Tune will wait ca. 400ms to let the axis settle and then start tuning that axis again. All axes are treated independently. +and yaw are tuned. If you stop rolling G-Tune will wait ca. 450ms to let the axis settle and then start tuning that axis again. All axes are treated independently. The easiest way to tune all axes at once is to do some air-jumps with the copter in Acro (RC centered and G-Tune activated... of course..). You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below). Yaw tune is disabled in any copter with less than 4 motors (like tricopters). @@ -24,17 +24,18 @@ TPA and G-Tune: It is not tested and will most likely not result into something ## Parameters and their function: - gtune_loP_rll = 20 [10..200] Lower limit of ROLL P during G-Tune. Note "20" means "2.0" in the GUI. - gtune_loP_ptch = 20 [10..200] Lower limit of PITCH P during G-Tune. Note "20" means "2.0" in the GUI. - gtune_loP_yw = 20 [10..200] Lower limit of YAW P during G-Tune. Note "20" means "2.0" in the GUI. - gtune_hiP_rll = 70 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. - gtune_hiP_ptch = 70 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. - gtune_hiP_yw = 70 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "70" means "7.0" in the GUI. - gtune_pwr = 0 [0..10] Strength of adjustment + gtune_loP_rll = 10 [0..200] Lower limit of ROLL P during G-Tune. Note "10" means "1.0" in the GUI. + gtune_loP_ptch = 10 [0..200] Lower limit of PITCH P during G-Tune. Note "10" means "1.0" in the GUI. + gtune_loP_yw = 10 [0..200] Lower limit of YAW P during G-Tune. Note "10" means "1.0" in the GUI. + gtune_hiP_rll = 100 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI. + gtune_hiP_ptch = 100 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI. + gtune_hiP_yw = 100 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI. + gtune_pwr = 0 [0..10] Strength of adjustment + gtune_settle_time = 450 [200..1000] Settle time in ms + gtune_average_cycles = 16 [8..128] Number of looptime cycles used for gyro average calcullation -So you have lower and higher limits for each P for every axis. The preset range (GUI: 2.0 - 7.0) is quiet broad to represent most setups. -If you want tighter ranges change them here. The gtune_loP_XXX can not be lower than "10" that means a P of "1.0" in the GUI. So you can not have "Zero P", -you are in maybe sluggish initial control. +So you have lower and higher limits for each P for every axis. The preset range (GUI: 1.0 - 10.0) is quiet broad to represent most setups. +If you want tighter or more loose ranges change them here. gtune_loP_XXX can be configured lower than "10" that means a P of "1.0" in the GUI. So you can have "Zero P" but you may get sluggish initial control. If you want to exclude one axis from the tuning you must set gtune_hiP_XXX to zero. Let's say you want to disable yaw tuning write in CLI "set gtune_hiP_yw = 0". Note: The MultiWii Wiki advises you to trim the yaw axis on your transmitter. If you have done so (yaw not neutral on your RC) yaw tuning will be disabled. diff --git a/src/main/config/config.c b/src/main/config/config.c index 8dc232f34f..1e612592f8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -192,13 +192,15 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pid5_maincuthz = 12; #ifdef GTUNE - pidProfile->gtune_lolimP[ROLL] = 20; // [10..200] Lower limit of ROLL P during G tune. - pidProfile->gtune_lolimP[PITCH] = 20; // [10..200] Lower limit of PITCH P during G tune. - pidProfile->gtune_lolimP[YAW] = 20; // [10..200] Lower limit of YAW P during G tune. - pidProfile->gtune_hilimP[ROLL] = 70; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis. - pidProfile->gtune_hilimP[PITCH] = 70; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis. - pidProfile->gtune_hilimP[YAW] = 70; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. + pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune. + pidProfile->gtune_lolimP[YAW] = 10; // [0..200] Lower limit of YAW P during G tune. + pidProfile->gtune_hilimP[ROLL] = 100; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_hilimP[PITCH] = 100; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis. + pidProfile->gtune_hilimP[YAW] = 100; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis. pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment + pidProfile->gtune_settle_time = 450; // [200..1000] Settle time in ms + pidProfile->gtune_average_cycles = 16; // [8..128] Number of looptime cycles used for gyro average calculation #endif } diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index 63c68afdb3..bcb3223403 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -44,6 +44,7 @@ #include "config/runtime_config.h" +extern uint16_t cycleTime; extern uint8_t motorCount; /* @@ -77,21 +78,29 @@ extern uint8_t motorCount; Gyrosetting 2000DPS GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s - pidProfile->gtune_lolimP[ROLL] = 20; [10..200] Lower limit of ROLL P during G tune. - pidProfile->gtune_lolimP[PITCH] = 20; [10..200] Lower limit of PITCH P during G tune. - pidProfile->gtune_lolimP[YAW] = 20; [10..200] Lower limit of YAW P during G tune. - pidProfile->gtune_hilimP[ROLL] = 70; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_hilimP[PITCH] = 70; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_hilimP[YAW] = 70; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis. - pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment + pidProfile->gtune_lolimP[ROLL] = 10; [0..200] Lower limit of ROLL P during G tune. + pidProfile->gtune_lolimP[PITCH] = 10; [0..200] Lower limit of PITCH P during G tune. + pidProfile->gtune_lolimP[YAW] = 10; [0..200] Lower limit of YAW P during G tune. + pidProfile->gtune_hilimP[ROLL] = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_hilimP[PITCH] = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_hilimP[YAW] = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis. + pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment + pidProfile->gtune_settle_time = 450; [200..1000] Settle time in ms + pidProfile->gtune_average_cycles = 16; [8..128] Number of looptime cycles used for gyro average calculation */ static pidProfile_t *pidProfile; -static int8_t time_skip[3]; +static int16_t delay_cycles; +static int16_t time_skip[3]; static int16_t OldError[3], result_P64[3]; static int32_t AvgGyro[3]; static bool floatPID; +void updateDelayCycles(void) +{ + delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime); +} + void init_Gtune(pidProfile_t *pidProfileToTune) { uint8_t i; @@ -99,9 +108,10 @@ void init_Gtune(pidProfile_t *pidProfileToTune) pidProfile = pidProfileToTune; if (pidProfile->pidController == 2) floatPID = true; // LuxFloat is using float values for PID settings else floatPID = false; + updateDelayCycles(); for (i = 0; i < 3; i++) { if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning - (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0; // Disable Yawtuning for everything below a quadcopter + (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0; // Disable yawtuning for everything below a quadcopter if (floatPID) { if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10); result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. @@ -110,7 +120,7 @@ void init_Gtune(pidProfile_t *pidProfileToTune) result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P. } OldError[i] = 0; - time_skip[i] = -125; + time_skip[i] = delay_cycles; } } @@ -118,9 +128,9 @@ void calculate_Gtune(uint8_t axis) { int16_t error, diff_G, threshP; - if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block Tuning on stickinput. Always allow Gtune on YAW, Roll & Pitch only in acromode + if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode OldError[axis] = 0; - time_skip[axis] = -125; // Some settletime after stick center. (125 + 16)* 3ms clycle = 423ms (ca.) + time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms } else { if (!time_skip[axis]) AvgGyro[axis] = 0; time_skip[axis]++; @@ -128,7 +138,7 @@ void calculate_Gtune(uint8_t axis) if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128); // Chop some jitter and average } - if (time_skip[axis] == 16) { // ca 48 ms + if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16. AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata time_skip[axis] = 0; if (axis == FD_YAW) { @@ -166,8 +176,8 @@ void calculate_Gtune(uint8_t axis) } #endif - if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID - else pidProfile->P8[axis] = newP; // new P value + if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID + else pidProfile->P8[axis] = newP; // new P value } OldError[axis] = error; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 21a6e048ff..52c50eb8e8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -70,9 +70,11 @@ typedef struct pidProfile_s { uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw #ifdef GTUNE - uint8_t gtune_lolimP[3]; // [10..200] Lower limit of P during G tune + uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis. - int8_t gtune_pwr; // [0..10] Strength of adjustment + uint8_t gtune_pwr; // [0..10] Strength of adjustment + uint16_t gtune_settle_time; // [200..1000] Settle time in ms + uint8_t gtune_average_cycles; // [8..128] Number of looptime cycles used for gyro average calculation #endif } pidProfile_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7ee8e43146..9822ef9f4f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -524,7 +524,9 @@ const clivalue_t valueTable[] = { { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], 0, 200 }, { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], 0, 200 }, { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], 0, 200 }, - { "gtune_pwr", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 }, + { "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 }, + { "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, 200, 1000 }, + { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, 8, 128 }, #endif #ifdef BLACKBOX From a8aad05c5ae86dff5d13028a522bcb2a2e737220 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 8 May 2015 20:19:32 +0200 Subject: [PATCH 10/17] Added additional yaw handling according to Spirre's PID2 testing. Flight tested with PID3 and PID5 (still functional without negative side effects) Additional code style updates of gtune.c --- src/main/flight/gtune.c | 62 +++++++++++++++++++++++++++++------------ 1 file changed, 44 insertions(+), 18 deletions(-) diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index bcb3223403..412f5cbfdc 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -106,17 +106,25 @@ void init_Gtune(pidProfile_t *pidProfileToTune) uint8_t i; pidProfile = pidProfileToTune; - if (pidProfile->pidController == 2) floatPID = true; // LuxFloat is using float values for PID settings - else floatPID = false; + if (pidProfile->pidController == 2) { + floatPID = true; // LuxFloat is using float values for PID settings + } else { + floatPID = false; + } updateDelayCycles(); for (i = 0; i < 3; i++) { - if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || // User config error disable axisis for tuning - (motorCount < 4 && i == FD_YAW)) pidProfile->gtune_hilimP[i] = 0; // Disable yawtuning for everything below a quadcopter + if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning + pidProfile->gtune_hilimP[i] = 0; // Disable yawtuning for everything below a quadcopter + } if (floatPID) { - if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10); + if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) { + pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10); + } result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. } else { - if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) pidProfile->P8[i] = pidProfile->gtune_lolimP[i]; + if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) { + pidProfile->P8[i] = pidProfile->gtune_lolimP[i]; + } result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P. } OldError[i] = 0; @@ -135,8 +143,11 @@ void calculate_Gtune(uint8_t axis) if (!time_skip[axis]) AvgGyro[axis] = 0; time_skip[axis]++; if (time_skip[axis] > 0) { - if (axis == FD_YAW) AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average - else AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128); // Chop some jitter and average + if (axis == FD_YAW) { + AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average + } else { + AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128); // Chop some jitter and average + } } if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16. AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata @@ -151,15 +162,25 @@ void calculate_Gtune(uint8_t axis) if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so diff_G = ABS(error) - ABS(OldError[axis]); if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) { - if (diff_G > threshP) result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side. - else { + if (diff_G > threshP) { + if (axis == FD_YAW) { + result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on PID2, give it some more to work with. + } else { + result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side. + } + } else { if (diff_G < -threshP) { - if (axis == FD_YAW) result_P64[axis] -= 64 + pidProfile->gtune_pwr; - else result_P64[axis] -= 32; + if (axis == FD_YAW) { + result_P64[axis] -= 64 + pidProfile->gtune_pwr; + } else { + result_P64[axis] -= 32; + } } } } else { - if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW + if (ABS(diff_G) > threshP && axis != FD_YAW) { + result_P64[axis] -= 32; // Don't use antiwobble for YAW + } } int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]); @@ -169,15 +190,20 @@ void calculate_Gtune(uint8_t axis) eventData.gtuneAxis = axis; eventData.gtuneGyroAVG = AvgGyro[axis]; - if (floatPID) eventData.gtuneNewP = newP / 10; - else eventData.gtuneNewP = newP; - + if (floatPID) { + eventData.gtuneNewP = newP / 10; + } else { + eventData.gtuneNewP = newP; + } blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); } #endif - if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID - else pidProfile->P8[axis] = newP; // new P value + if (floatPID) { + pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID + } else { + pidProfile->P8[axis] = newP; // new P value + } } OldError[axis] = error; } From afed9a5bba75bcce71dddb12b44ad0982ffd0912 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 9 May 2015 16:41:26 +0200 Subject: [PATCH 11/17] G-Tune fix for PID controller 2 (LuxFloat) --- src/main/flight/gtune.c | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index 412f5cbfdc..6fa2b1950c 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -107,18 +107,18 @@ void init_Gtune(pidProfile_t *pidProfileToTune) pidProfile = pidProfileToTune; if (pidProfile->pidController == 2) { - floatPID = true; // LuxFloat is using float values for PID settings + floatPID = true; // LuxFloat is using float values for PID settings } else { floatPID = false; } updateDelayCycles(); for (i = 0; i < 3; i++) { if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning - pidProfile->gtune_hilimP[i] = 0; // Disable yawtuning for everything below a quadcopter + pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter } if (floatPID) { - if((pidProfile->P_f[i] * 10) < pidProfile->gtune_lolimP[i]) { - pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10); + if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) { + pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f); } result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P. } else { @@ -164,7 +164,7 @@ void calculate_Gtune(uint8_t axis) if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) { if (diff_G > threshP) { if (axis == FD_YAW) { - result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on PID2, give it some more to work with. + result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on float PID, give it some more to work with. } else { result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side. } @@ -190,17 +190,13 @@ void calculate_Gtune(uint8_t axis) eventData.gtuneAxis = axis; eventData.gtuneGyroAVG = AvgGyro[axis]; - if (floatPID) { - eventData.gtuneNewP = newP / 10; - } else { - eventData.gtuneNewP = newP; - } + eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10 blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData); } #endif if (floatPID) { - pidProfile->P_f[axis] = (float)(newP / 10); // new P value for float PID + pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID } else { pidProfile->P8[axis] = newP; // new P value } From 4bed8bc78a7725a2aef7c322874dc9c4b1c2592c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 11 May 2015 06:39:37 +0200 Subject: [PATCH 12/17] Add G-Tune for NAZE32PRO target --- src/main/target/NAZE32PRO/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 6b35128d32..608b90e0d4 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -45,6 +45,7 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE +#define GTUNE #define USE_SERVOS #define USE_CLI From 31dd2fc223f6a36c6ef866e8ccbc5c115711aad6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 12 May 2015 16:16:43 +0200 Subject: [PATCH 13/17] G-Tune documentation update --- docs/Modes.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Modes.md b/docs/Modes.md index 115f273e02..8e74f1fdee 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -28,6 +28,7 @@ auxillary receiver channels and other events such as failsafe detection. | 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs | | 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) | | 26 | 25 | BLACKBOX | Enable BlackBox logging | +| 27 | 26 | GTUNE | G-Tune - auto tuning of Pitch/Roll/Yaw P values | ## Mode details From edf08648bce01bdb31f6880af8df230536fd53f1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 30 May 2015 02:04:46 +0200 Subject: [PATCH 14/17] Replace gyroData with gyroADC in gtune.c after rebase --- src/main/flight/gtune.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index 6fa2b1950c..43fcf37f36 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -32,6 +32,7 @@ #include "drivers/accgyro.h" #include "sensors/sensors.h" +#include "sensors/gyro.h" #include "sensors/acceleration.h" #include "flight/pid.h" @@ -144,9 +145,9 @@ void calculate_Gtune(uint8_t axis) time_skip[axis]++; if (time_skip[axis] > 0) { if (axis == FD_YAW) { - AvgGyro[axis] += 32 * ((int16_t)gyroData[axis] / 32); // Chop some jitter and average + AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32); // Chop some jitter and average } else { - AvgGyro[axis] += 128 * ((int16_t)gyroData[axis] / 128); // Chop some jitter and average + AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128); // Chop some jitter and average } } if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16. From 72ff296850a58abe664ce6be5f8b5faa0f100536 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 4 Jun 2015 18:25:40 +0200 Subject: [PATCH 15/17] Enable G-Tune for CC3D (but not included in the OPBL version) --- src/main/target/CC3D/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a79dab592a..136a7171a1 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -113,6 +113,7 @@ #define SERIAL_RX #define SONAR #define AUTOTUNE +#define GTUNE #define USE_SERVOS #define USE_CLI @@ -120,6 +121,7 @@ // disabled some features for OPBL build due to code size. #undef AUTOTUNE #undef DISPLAY +#undef GTUNE #undef SONAR #define SKIP_CLI_COMMAND_HELP #endif From e42ed1ad7037d453b59c2454161e0f25cdf63ea7 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 29 Jun 2015 22:34:38 +0200 Subject: [PATCH 16/17] Update to integrate with BorisB filters after rebase. Documentation update --- docs/PID tuning.md | 3 ++- src/main/config/config.c | 1 - src/main/io/serial_cli.c | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 972584c5e8..a125739c13 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -152,7 +152,8 @@ PID Controller 5 is an port of the PID controller from the Harakiri firmware. The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. There are two additional settings which are configurable via the CLI in Harakiri: - set pid5_maincuthz = 12 [1-50Hz] Cut Off Frequency for D term of main Pid controller + set dterm_cut_hz = 0 [1-50Hz] Cut Off Frequency for D term of main PID controller + (default of 0 equals to 12Hz which was the hardcoded setting in previous Cleanflight versions) set pid5_oldyw = 0 [0/1] 0 = multiwii 2.3 yaw (default), 1 = older yaw The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. diff --git a/src/main/config/config.c b/src/main/config/config.c index 1e612592f8..5eab90d1e5 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -189,7 +189,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->H_sensitivity = 75; pidProfile->pid5_oldyw = 0; - pidProfile->pid5_maincuthz = 12; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9822ef9f4f..41200fdeb1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -515,7 +515,6 @@ const clivalue_t valueTable[] = { { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, { "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 }, - { "pid5_maincuthz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 }, #ifdef GTUNE { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 }, From f15cedd057c347dec3678216b7af01b2d1c743ec Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 4 Aug 2015 15:09:38 +0200 Subject: [PATCH 17/17] Fixes after rebase --- src/main/blackbox/blackbox_fielddefs.h | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index fcd087584d..47c128c1e8 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -149,18 +149,16 @@ typedef struct flightLogEvent_inflightAdjustment_t { float newFloatValue; } flightLogEvent_inflightAdjustment_t; -<<<<<<< Upstream, based on origin/master typedef struct flightLogEvent_loggingResume_t { uint32_t logIteration; uint32_t currentTime; } flightLogEvent_loggingResume_t; -======= + typedef struct flightLogEvent_gtuneCycleResult_t { uint8_t gtuneAxis; int32_t gtuneGyroAVG; int16_t gtuneNewP; } flightLogEvent_gtuneCycleResult_t; ->>>>>>> 6f60c52 Add BlackBox recording for G-Tune typedef union flightLogEventData_t { @@ -169,11 +167,8 @@ typedef union flightLogEventData_t flightLogEvent_autotuneCycleResult_t autotuneCycleResult; flightLogEvent_autotuneTargets_t autotuneTargets; flightLogEvent_inflightAdjustment_t inflightAdjustment; -<<<<<<< Upstream, based on origin/master flightLogEvent_loggingResume_t loggingResume; -======= flightLogEvent_gtuneCycleResult_t gtuneCycleResult; ->>>>>>> 6f60c52 Add BlackBox recording for G-Tune } flightLogEventData_t; typedef struct flightLogEvent_t