mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 14:55:21 +03:00
Cleanup mw.c // Remove unnecessary functions
This commit is contained in:
borisbstyle
parent
979096f333
commit
4e3704374a
17 changed files with 46 additions and 348 deletions
3
Makefile
3
Makefile
|
|
@ -524,7 +524,7 @@ COMMON_SRC = \
|
|||
fc/config.c \
|
||||
fc/fc_tasks.c \
|
||||
fc/fc_msp.c \
|
||||
fc/mw.c \
|
||||
fc/fc_main.c \
|
||||
fc/rc_controls.c \
|
||||
fc/rc_curves.c \
|
||||
fc/runtime_config.c \
|
||||
|
|
@ -586,7 +586,6 @@ HIGHEND_SRC = \
|
|||
drivers/serial_escserial.c \
|
||||
drivers/serial_softserial.c \
|
||||
drivers/sonar_hcsr04.c \
|
||||
flight/gtune.c \
|
||||
flight/navigation.c \
|
||||
flight/gps_conversion.c \
|
||||
io/dashboard.c \
|
||||
|
|
|
|||
|
|
@ -1258,7 +1258,7 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle);
|
||||
|
||||
// Betaflight PID controller parameters
|
||||
BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", currentProfile->pidProfile.itermThrottleGain);
|
||||
BLACKBOX_PRINT_HEADER_LINE("itermThrottleThreshold:%d", currentProfile->pidProfile.itermThrottleThreshold);
|
||||
BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentProfile->pidProfile.setpointRelaxRatio);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", currentProfile->pidProfile.yawRateAccelLimit);
|
||||
|
|
@ -1330,11 +1330,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
|
|||
blackboxWriteSignedVB(data->inflightAdjustment.newValue);
|
||||
}
|
||||
break;
|
||||
case FLIGHT_LOG_EVENT_GTUNE_RESULT:
|
||||
blackboxWrite(data->gtuneCycleResult.gtuneAxis);
|
||||
blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
|
||||
blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
|
||||
break;
|
||||
case FLIGHT_LOG_EVENT_LOGGING_RESUME:
|
||||
blackboxWriteUnsignedVB(data->loggingResume.logIteration);
|
||||
blackboxWriteUnsignedVB(data->loggingResume.currentTime);
|
||||
|
|
|
|||
|
|
@ -106,7 +106,6 @@ typedef enum FlightLogEvent {
|
|||
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
|
||||
FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
|
||||
FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
|
||||
FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
|
||||
FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status.
|
||||
FLIGHT_LOG_EVENT_LOG_END = 255
|
||||
} FlightLogEvent;
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
#define PG_CONTROL_RATE_PROFILES 12 // struct OK, needs to be split out of rc_controls.h into rate_profile.h
|
||||
#define PG_SERIAL_CONFIG 13 // struct OK
|
||||
#define PG_PID_PROFILE 14 // struct OK, CF differences
|
||||
#define PG_GTUNE_CONFIG 15 // is GTUNE still being used?
|
||||
#define PG_ARMING_CONFIG 16 // structs OK, CF naming differences
|
||||
#define PG_TRANSPONDER_CONFIG 17 // struct OK
|
||||
#define PG_SYSTEM_CONFIG 18 // just has i2c_highspeed
|
||||
|
|
|
|||
|
|
@ -168,8 +168,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->pidSumLimit = PIDSUM_LIMIT;
|
||||
pidProfile->yaw_lpf_hz = 0;
|
||||
pidProfile->rollPitchItermIgnoreRate = 130;
|
||||
pidProfile->yawItermIgnoreRate = 32;
|
||||
pidProfile->rollPitchItermIgnoreRate = 200;
|
||||
pidProfile->yawItermIgnoreRate = 55;
|
||||
pidProfile->dterm_filter_type = FILTER_BIQUAD;
|
||||
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
|
||||
pidProfile->dterm_notch_hz = 260;
|
||||
|
|
@ -182,20 +182,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->dtermSetpointWeight = 200;
|
||||
pidProfile->yawRateAccelLimit = 220;
|
||||
pidProfile->rateAccelLimit = 0;
|
||||
pidProfile->itermThrottleGain = 0;
|
||||
pidProfile->itermThrottleThreshold = 350;
|
||||
pidProfile->levelSensitivity = 2.0f;
|
||||
|
||||
#ifdef GTUNE
|
||||
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
|
||||
}
|
||||
|
||||
void resetProfile(profile_t *profile)
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@
|
|||
#include "flight/servos.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gtune.h"
|
||||
#include "flight/altitudehold.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
|
|
@ -109,30 +108,6 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
|
|||
saveConfigAndNotify();
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
|
||||
void updateGtuneState(void)
|
||||
{
|
||||
static bool GTuneWasUsed = false;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
|
||||
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) && ARMING_FLAG(ARMED)) {
|
||||
DISABLE_FLIGHT_MODE(GTUNE_MODE);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
bool isCalibrating()
|
||||
{
|
||||
#ifdef BARO
|
||||
|
|
@ -199,20 +174,45 @@ void scaleRcCommandToFpvCamAngle(void) {
|
|||
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
|
||||
}
|
||||
|
||||
#define THROTTLE_BUFFER_MAX 20
|
||||
#define THROTTLE_DELTA_MS 100
|
||||
|
||||
void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
|
||||
static int index;
|
||||
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
|
||||
const int rxRefreshRateMs = rxRefreshRate / 1000;
|
||||
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
|
||||
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
|
||||
|
||||
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
|
||||
if (index >= indexMax)
|
||||
index = 0;
|
||||
|
||||
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
|
||||
|
||||
if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
|
||||
pidResetErrorGyroState();
|
||||
}
|
||||
|
||||
void processRcCommand(void)
|
||||
{
|
||||
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
|
||||
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
|
||||
static int16_t factor, rcInterpolationFactor;
|
||||
static uint16_t currentRxRefreshRate;
|
||||
uint16_t rxRefreshRate;
|
||||
bool readyToCalculateRate = false;
|
||||
|
||||
if (rxConfig()->rcInterpolation || flightModeFlags) {
|
||||
if (isRXDataNew) {
|
||||
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
|
||||
checkForThrottleErrorResetState(currentRxRefreshRate);
|
||||
}
|
||||
|
||||
if (rxConfig()->rcInterpolation || flightModeFlags) {
|
||||
// Set RC refresh rate for sampling and channels to filter
|
||||
switch (rxConfig()->rcInterpolation) {
|
||||
switch(rxConfig()->rcInterpolation) {
|
||||
case(RC_SMOOTHING_AUTO):
|
||||
rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
|
||||
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
|
||||
break;
|
||||
case(RC_SMOOTHING_MANUAL):
|
||||
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
|
||||
|
|
@ -223,6 +223,7 @@ void processRcCommand(void)
|
|||
rxRefreshRate = rxGetRefreshRate();
|
||||
}
|
||||
|
||||
if (isRXDataNew) {
|
||||
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
|
||||
|
||||
if (debugMode == DEBUG_RC_INTERPOLATION) {
|
||||
|
|
@ -49,7 +49,7 @@
|
|||
#include "drivers/serial_escserial.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/mw.h"
|
||||
#include "fc/fc_main.h"
|
||||
#include "fc/fc_msp.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
|
@ -401,10 +401,6 @@ void initActiveBoxIds(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef GTUNE
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
|
||||
#endif
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
|
||||
|
|
@ -1167,7 +1163,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU8(dst, currentProfile->pidProfile.dtermSetpointWeight);
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU8(dst, currentProfile->pidProfile.itermThrottleGain);
|
||||
sbufWriteU8(dst, 0); // reserved
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit);
|
||||
break;
|
||||
|
|
@ -1515,7 +1511,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
currentProfile->pidProfile.dtermSetpointWeight = sbufReadU8(src);
|
||||
sbufReadU8(src); // reserved
|
||||
sbufReadU8(src); // reserved
|
||||
currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
|
||||
sbufReadU8(src); // reserved
|
||||
currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
|
||||
currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
|
||||
pidInitConfig(¤tProfile->pidProfile);
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
#include "fc/config.h"
|
||||
#include "fc/fc_msp.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
#include "fc/mw.h"
|
||||
#include "fc/fc_main.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@
|
|||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/mw.h"
|
||||
#include "fc/fc_main.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_curves.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
|
|
|||
|
|
@ -42,8 +42,7 @@ typedef enum {
|
|||
UNUSED_MODE = (1 << 7), // old autotune
|
||||
PASSTHRU_MODE = (1 << 8),
|
||||
SONAR_MODE = (1 << 9),
|
||||
FAILSAFE_MODE = (1 << 10),
|
||||
GTUNE_MODE = (1 << 11)
|
||||
FAILSAFE_MODE = (1 << 10)
|
||||
} flightModeFlags_e;
|
||||
|
||||
extern uint16_t flightModeFlags;
|
||||
|
|
|
|||
|
|
@ -1,211 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <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/gyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
|
||||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
||||
extern uint16_t cycleTime;
|
||||
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] = 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 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;
|
||||
|
||||
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]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
|
||||
pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter
|
||||
}
|
||||
if (floatPID) {
|
||||
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 {
|
||||
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] = delay_cycles;
|
||||
}
|
||||
}
|
||||
|
||||
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 stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
|
||||
OldError[axis] = 0;
|
||||
time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms
|
||||
} 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)gyroADC[axis] / 32); // Chop some jitter and average
|
||||
} else {
|
||||
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.
|
||||
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) {
|
||||
if (axis == FD_YAW) {
|
||||
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.
|
||||
}
|
||||
} 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
|
||||
}
|
||||
}
|
||||
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; // 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.0f; // new P value for float PID
|
||||
} else {
|
||||
pidProfile->P8[axis] = newP; // new P value
|
||||
}
|
||||
}
|
||||
OldError[axis] = error;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
void init_Gtune(pidProfile_t *pidProfileToTune);
|
||||
void calculate_Gtune(uint8_t axis);
|
||||
|
|
@ -34,7 +34,6 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "flight/gtune.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
|
@ -183,24 +182,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
}
|
||||
}
|
||||
|
||||
// Yet Highly experimental and under test and development
|
||||
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
|
||||
static float kiThrottleGain = 1.0f;
|
||||
if (pidProfile->itermThrottleGain) {
|
||||
const uint16_t maxLoopCount = 20000 / targetPidLooptime;
|
||||
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
|
||||
static int16_t previousThrottle;
|
||||
static uint16_t loopIncrement;
|
||||
|
||||
if (loopIncrement >= maxLoopCount) {
|
||||
kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
|
||||
previousThrottle = rcCommand[THROTTLE];
|
||||
loopIncrement = 0;
|
||||
} else {
|
||||
loopIncrement++;
|
||||
}
|
||||
}
|
||||
|
||||
// ----------PID controller----------
|
||||
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
|
|
@ -247,10 +228,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
// Reduce strong Iterm accumulation during higher stick inputs
|
||||
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
|
||||
const float itermScaler = setpointRateScaler * kiThrottleGain;
|
||||
|
||||
float ITerm = previousGyroIf[axis];
|
||||
ITerm += Ki[axis] * errorRate * dT * itermScaler;;
|
||||
ITerm += Ki[axis] * errorRate * dT * setpointRateScaler;
|
||||
// limit maximum integrator value to prevent WindUp
|
||||
ITerm = constrainf(ITerm, -250.0f, 250.0f);
|
||||
previousGyroIf[axis] = ITerm;
|
||||
|
|
@ -291,12 +271,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
// Disable PID control at zero throttle
|
||||
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BLACKBOX
|
||||
axisPID_P[axis] = PTerm;
|
||||
axisPID_I[axis] = ITerm;
|
||||
|
|
|
|||
|
|
@ -74,20 +74,12 @@ typedef struct pidProfile_s {
|
|||
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
|
||||
|
||||
// Betaflight PID controller parameters
|
||||
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
|
||||
uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms
|
||||
uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect
|
||||
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
|
||||
uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms
|
||||
uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
|
||||
float levelSensitivity;
|
||||
|
||||
#ifdef GTUNE
|
||||
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.
|
||||
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;
|
||||
|
||||
typedef struct pidConfig_s {
|
||||
|
|
|
|||
|
|
@ -757,18 +757,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &gpsProfile()->nav_slew_rate, .config.minmax = { 0, 100 } },
|
||||
#endif
|
||||
|
||||
#ifdef GTUNE
|
||||
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } },
|
||||
{ "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } },
|
||||
{ "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 } },
|
||||
{ "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 } },
|
||||
{ "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 } },
|
||||
{ "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 } },
|
||||
{ "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 } },
|
||||
{ "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 } },
|
||||
{ "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } },
|
||||
#endif
|
||||
|
||||
#ifdef BEEPER
|
||||
{ "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isInverted, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &beeperConfig()->isOpenDrain, .config.lookup = { TABLE_OFF_ON } },
|
||||
|
|
@ -906,7 +894,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } },
|
||||
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
|
||||
{ "iterm_throttle_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } },
|
||||
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
|
||||
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
|
||||
{ "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@
|
|||
#include "drivers/rx_pwm.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/mw.h"
|
||||
#include "fc/fc_main.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue