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
This commit is contained in:
Michael Jakob
parent
b20dc77a74
commit
43f5792a61
1
Makefile
1
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 \
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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
|
||||
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
void init_Gtune(pidProfile_t *pidProfileToTune);
|
||||
void calculate_Gtune(uint8_t axis);
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ typedef enum {
|
|||
BOXSERVO3,
|
||||
BOXBLACKBOX,
|
||||
BOXFAILSAFE,
|
||||
BOXGTUNE,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 },
|
||||
|
|
|
|||
|
|
@ -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 |
|
||||
|
|
|
|||
|
|
@ -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)) {
|
||||
|
|
|
|||
|
|
@ -115,6 +115,7 @@
|
|||
//#define GPS
|
||||
//#define DISPLAY
|
||||
#define AUTOTUNE
|
||||
#define GTUNE
|
||||
#define USE_SERVOS
|
||||
#define USE_CLI
|
||||
|
||||
|
|
|
|||
|
|
@ -175,6 +175,7 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define GTUNE
|
||||
#define USE_SERVOS
|
||||
#define USE_CLI
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue