From 66c6a5f877081bf8351fb8e3edd71d41f24db136 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Mon, 27 Dec 2021 22:52:15 -0500 Subject: [PATCH] AC_AutoTune: reorder defines and remove unused in heli --- libraries/AC_AutoTune/AC_AutoTune.cpp | 40 +---------- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 19 +++--- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 76 +++++++++++++-------- 3 files changed, 58 insertions(+), 77 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index d5d93460c5..f8908c73b2 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -2,41 +2,6 @@ #include #include -/* - * autotune support for multicopters - * - * Instructions: - * 1) Set up one flight mode switch position to be AltHold. - * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch. - * 3) Ensure the ch7 or ch8 switch is in the LOW position. - * 4) Wait for a calm day and go to a large open area. - * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. - * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: - * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. - * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). - * When you release the sticks it will continue auto tuning where it left off. - * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. - * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered. - * 7) When the tune completes the vehicle will change back to the original PID gains. - * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains. - * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains. - * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently. - * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm - * - * What it's doing during each "twitch": - * a) invokes 90 deg/sec rate request - * b) records maximum "forward" roll rate and bounce back rate - * c) when copter reaches 20 degrees or 1 second has passed, it commands level - * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P - * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec) - * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec) - * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec) - * h) invokes a 20deg angle request on roll or pitch - * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg) - * j) decreases stab P by 25% - * - */ - #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) # define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg) @@ -49,10 +14,7 @@ #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level #define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level #define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users -#define AUTOTUNE_ANGLE_MAX_RLLPIT 30.0f // maximum allowable angle during testing - -// second table of user settable parameters for quadplanes, this -// allows us to go beyond the 64 parameter limit +#define AUTOTUNE_ANGLE_MAX_RLLPIT 30.0f // maximum allowable angle in degrees during testing AC_AutoTune::AC_AutoTune() { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 8704be372a..10c63325ae 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -19,34 +19,31 @@ #include "AC_AutoTune_Heli.h" -#define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 2000 // target roll/pitch angle during AUTOTUNE FeedForward rate test -#define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test -#define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing -#define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after testing -#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing -#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing #define AUTOTUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term #define AUTOTUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term #define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing +#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing +#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing #define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value -#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value -#define AUTOTUNE_RLPF_MAX 20.0f // maximum Rate Yaw filter value #define AUTOTUNE_RP_MIN 0.001f // minimum Rate P value #define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value #define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value #define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value #define AUTOTUNE_RFF_MAX 0.5f // maximum Stab P value #define AUTOTUNE_RFF_MIN 0.025f // maximum Stab P value -#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in - #define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning #define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration + +#define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 2000 // target roll/pitch angle during AUTOTUNE FeedForward rate test +#define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test +#define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing +#define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after testing #define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch #define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw -#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_SEQ_BITMASK_VFF 1 #define AUTOTUNE_SEQ_BITMASK_RATE_D 2 diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 606480f4ed..51a017fe09 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1,20 +1,39 @@ -/* - This program 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. +#include "AC_AutoTune_Multi.h" - This program 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 this program. If not, see . - */ /* - support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall - Converted to a library by Andrew Tridgell + * autotune support for multicopters + * + * + * Instructions: + * 1) Set up one flight mode switch position to be AltHold. + * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch. + * 3) Ensure the ch7 or ch8 switch is in the LOW position. + * 4) Wait for a calm day and go to a large open area. + * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. + * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: + * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. + * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). + * When you release the sticks it will continue auto tuning where it left off. + * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. + * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered. + * 7) When the tune completes the vehicle will change back to the original PID gains. + * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains. + * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains. + * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently. + * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm + * + * What it's doing during each "twitch": + * a) invokes 90 deg/sec rate request + * b) records maximum "forward" roll rate and bounce back rate + * c) when copter reaches 20 degrees or 1 second has passed, it commands level + * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P + * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec) + * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec) + * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec) + * h) invokes a 20deg angle request on roll or pitch + * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg) + * j) decreases stab P by 25% + * */ #define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term @@ -30,24 +49,27 @@ #define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value #define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value #define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value +#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw +#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in -#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration + +// roll and pitch axes #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step + +// yaw axis #define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step #define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step -#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw - -#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning -#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning -#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration -#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration -#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw -#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning - -#include "AC_AutoTune_Multi.h" +// second table of user settable parameters for quadplanes, this +// allows us to go beyond the 64 parameter limit const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = { // @Param: AXES