mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15:26 +03:00
PID: LuxFloat rewritten from scratch. New I-term back-tracking for anti-windup prevention
AIRMODE: Air mode anti-windup improvements PID: Remove integer PID (leave only one PID controller), remove pid_controller setting, remove float-point PID parameters (scale integer parameters to use in float-point PIDC) PID: New set of PID tuning multipliers for better tuning range PID: Make max_angle_inclination a PID-profile variable PID: Reinstate yaw_p_limit PID: Switch to a noise-robust differentiator Remove GTUNE PID: Use same self-leveling strength for ANGLE and HORIZON PID: Self-leveling LPF to smooth out leveling response to rapid attitude or target angle change
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
6269f55a0e
commit
f1aa9e6f44
34 changed files with 250 additions and 826 deletions
12
Makefile
12
Makefile
|
|
@ -266,12 +266,11 @@ COMMON_SRC = build_config.c \
|
|||
$(DEVICE_STDPERIPH_SRC)
|
||||
|
||||
HIGHEND_SRC = \
|
||||
flight/gtune.c \
|
||||
flight/navigation_rewrite.c \
|
||||
flight/navigation_rewrite_multicopter.c \
|
||||
flight/navigation_rewrite_fixedwing.c \
|
||||
flight/navigation_rewrite_pos_estimator.c \
|
||||
flight/navigation_rewrite_geo.c \
|
||||
flight/navigation_rewrite.c \
|
||||
flight/navigation_rewrite_multicopter.c \
|
||||
flight/navigation_rewrite_fixedwing.c \
|
||||
flight/navigation_rewrite_pos_estimator.c \
|
||||
flight/navigation_rewrite_geo.c \
|
||||
flight/gps_conversion.c \
|
||||
common/colorconversion.c \
|
||||
io/gps.c \
|
||||
|
|
@ -440,7 +439,6 @@ CJMCU_SRC = \
|
|||
drivers/timer.c \
|
||||
drivers/timer_stm32f10x.c \
|
||||
hardware_revision.c \
|
||||
flight/gtune.c \
|
||||
blackbox/blackbox.c \
|
||||
blackbox/blackbox_io.c \
|
||||
$(COMMON_SRC)
|
||||
|
|
|
|||
|
|
@ -425,11 +425,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
|
||||
return currentProfile->pidProfile.D_f[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
} else {
|
||||
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
}
|
||||
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MAG:
|
||||
#ifdef MAG
|
||||
|
|
@ -1279,11 +1275,6 @@ 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);
|
||||
blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
|
||||
blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
|
||||
break;
|
||||
case FLIGHT_LOG_EVENT_LOGGING_RESUME:
|
||||
blackboxWriteUnsignedVB(data->loggingResume.logIteration);
|
||||
blackboxWriteUnsignedVB(data->loggingResume.currentTime);
|
||||
|
|
|
|||
|
|
@ -105,7 +105,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_LOG_END = 255
|
||||
} FlightLogEvent;
|
||||
|
||||
|
|
@ -127,17 +126,10 @@ typedef struct flightLogEvent_loggingResume_s {
|
|||
|
||||
#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128
|
||||
|
||||
typedef struct flightLogEvent_gtuneCycleResult_s {
|
||||
uint8_t gtuneAxis;
|
||||
int32_t gtuneGyroAVG;
|
||||
int16_t gtuneNewP;
|
||||
} flightLogEvent_gtuneCycleResult_t;
|
||||
|
||||
typedef union flightLogEventData_u {
|
||||
flightLogEvent_syncBeep_t syncBeep;
|
||||
flightLogEvent_inflightAdjustment_t inflightAdjustment;
|
||||
flightLogEvent_loggingResume_t loggingResume;
|
||||
flightLogEvent_gtuneCycleResult_t gtuneCycleResult;
|
||||
} flightLogEventData_t;
|
||||
|
||||
typedef struct flightLogEvent_s {
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ float filterApplyBiQuad(float sample, biquad_t *state)
|
|||
}
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT)
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT)
|
||||
{
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ typedef struct biquad_s {
|
|||
float x1, x2, y1, y2;
|
||||
} biquad_t;
|
||||
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
|
||||
void filterResetPt1(filterStatePt1_t *filter, float input);
|
||||
|
||||
void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
|
||||
|
|
|
|||
|
|
@ -149,17 +149,15 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynam
|
|||
|
||||
void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
pidProfile->pidController = PID_CONTROLLER_MWREWRITE;
|
||||
|
||||
pidProfile->P8[ROLL] = 40;
|
||||
pidProfile->I8[ROLL] = 30;
|
||||
pidProfile->D8[ROLL] = 23;
|
||||
pidProfile->P8[PITCH] = 40;
|
||||
pidProfile->I8[PITCH] = 30;
|
||||
pidProfile->D8[PITCH] = 23;
|
||||
pidProfile->P8[YAW] = 85;
|
||||
pidProfile->I8[YAW] = 45;
|
||||
pidProfile->D8[YAW] = 0;
|
||||
pidProfile->P8[ROLL] = 56;
|
||||
pidProfile->I8[ROLL] = 120;
|
||||
pidProfile->D8[ROLL] = 80;
|
||||
pidProfile->P8[PITCH] = 56;
|
||||
pidProfile->I8[PITCH] = 120;
|
||||
pidProfile->D8[PITCH] = 80;
|
||||
pidProfile->P8[YAW] = 140; // 3.5 * 40
|
||||
pidProfile->I8[YAW] = 160; // 4.0 * 40
|
||||
pidProfile->D8[YAW] = 40; // 0.01 * 4000
|
||||
pidProfile->P8[PIDALT] = 50; // NAV_POS_Z_P * 100
|
||||
pidProfile->I8[PIDALT] = 0; // not used
|
||||
pidProfile->D8[PIDALT] = 0; // not used
|
||||
|
|
@ -172,9 +170,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->P8[PIDNAVR] = 14; // FW_NAV_P * 100
|
||||
pidProfile->I8[PIDNAVR] = 2; // FW_NAV_I * 100
|
||||
pidProfile->D8[PIDNAVR] = 8; // FW_NAV_D * 100
|
||||
pidProfile->P8[PIDLEVEL] = 20;
|
||||
pidProfile->I8[PIDLEVEL] = 20;
|
||||
pidProfile->D8[PIDLEVEL] = 100;
|
||||
pidProfile->P8[PIDLEVEL] = 160; // Self-level strength * 40 (4 * 40)
|
||||
pidProfile->I8[PIDLEVEL] = 10; // Self-leveing low-pass frequency (0 - disabled)
|
||||
pidProfile->D8[PIDLEVEL] = 75; // 75% horizon strength
|
||||
pidProfile->P8[PIDMAG] = 40;
|
||||
pidProfile->P8[PIDVEL] = 100; // NAV_VEL_Z_P * 100
|
||||
pidProfile->I8[PIDVEL] = 50; // NAV_VEL_Z_I * 100
|
||||
|
|
@ -182,34 +180,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
|
||||
pidProfile->acc_soft_lpf_hz = 15;
|
||||
pidProfile->gyro_soft_lpf_hz = 60;
|
||||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->dterm_lpf_hz = 30;
|
||||
|
||||
pidProfile->P_f[ROLL] = 1.4f; // new PID with preliminary defaults test carefully
|
||||
pidProfile->I_f[ROLL] = 0.3f;
|
||||
pidProfile->D_f[ROLL] = 0.02f;
|
||||
pidProfile->P_f[PITCH] = 1.4f;
|
||||
pidProfile->I_f[PITCH] = 0.3f;
|
||||
pidProfile->D_f[PITCH] = 0.02f;
|
||||
pidProfile->P_f[YAW] = 3.5f;
|
||||
pidProfile->I_f[YAW] = 0.4f;
|
||||
pidProfile->D_f[YAW] = 0.01f;
|
||||
pidProfile->A_level = 4.0f;
|
||||
pidProfile->H_level = 4.0f;
|
||||
pidProfile->H_sensitivity = 75;
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
|
||||
#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
|
||||
pidProfile->max_angle_inclination = 300; // 30 degrees
|
||||
}
|
||||
|
||||
#ifdef NAV
|
||||
|
|
@ -464,7 +439,6 @@ static void resetConf(void)
|
|||
masterConfig.boardAlignment.pitchDeciDegrees = 0;
|
||||
masterConfig.boardAlignment.yawDeciDegrees = 0;
|
||||
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
masterConfig.max_angle_inclination = 300; // 30 degrees
|
||||
masterConfig.yaw_control_direction = 1;
|
||||
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
|
||||
|
||||
|
|
@ -602,8 +576,6 @@ static void resetConf(void)
|
|||
#if defined(COLIBRI_RACE)
|
||||
masterConfig.looptime = 1000;
|
||||
|
||||
currentProfile->pidProfile.pidController = 1;
|
||||
|
||||
masterConfig.rxConfig.rcmap[0] = 1;
|
||||
masterConfig.rxConfig.rcmap[1] = 2;
|
||||
masterConfig.rxConfig.rcmap[2] = 3;
|
||||
|
|
@ -635,7 +607,6 @@ static void resetConf(void)
|
|||
masterConfig.escAndServoConfig.maxthrottle = 2000;
|
||||
masterConfig.motor_pwm_rate = 32000;
|
||||
masterConfig.looptime = 2000;
|
||||
currentProfile->pidProfile.pidController = 3;
|
||||
currentProfile->pidProfile.P8[ROLL] = 36;
|
||||
currentProfile->pidProfile.P8[PITCH] = 36;
|
||||
masterConfig.failsafeConfig.failsafe_delay = 2;
|
||||
|
|
@ -769,9 +740,6 @@ void activateConfig(void)
|
|||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||
#endif
|
||||
|
||||
currentProfile->pidProfile.pidController = constrain(currentProfile->pidProfile.pidController, 1, 2); // This should limit to USED values only
|
||||
pidSetController(currentProfile->pidProfile.pidController);
|
||||
|
||||
useFailsafeConfig(&masterConfig.failsafeConfig);
|
||||
|
||||
setAccelerationZero(&masterConfig.accZero);
|
||||
|
|
|
|||
|
|
@ -55,6 +55,7 @@ typedef struct master_t {
|
|||
uint16_t dcm_ki_acc; // DCM filter integral gain ( x 10000) for accelerometer
|
||||
uint16_t dcm_kp_mag; // DCM filter proportional gain ( x 10000) for magnetometer and GPS heading
|
||||
uint16_t dcm_ki_mag; // DCM filter integral gain ( x 10000) for magnetometer and GPS heading
|
||||
|
||||
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
|
||||
gyroConfig_t gyroConfig;
|
||||
|
|
@ -64,7 +65,6 @@ typedef struct master_t {
|
|||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
uint8_t baro_hardware; // Barometer hardware to use
|
||||
|
||||
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
|
||||
flightDynamicsTrims_t accZero; // Accelerometer offset
|
||||
flightDynamicsTrims_t accGain; // Accelerometer gain to read exactly 1G
|
||||
flightDynamicsTrims_t magZero; // Compass offset
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ typedef enum {
|
|||
UNUSED_MODE = (1 << 7), // old autotune
|
||||
PASSTHRU_MODE = (1 << 8),
|
||||
FAILSAFE_MODE = (1 << 10),
|
||||
GTUNE_MODE = (1 << 11),
|
||||
UNUSED_MODE2 = (1 << 11), // old G-Tune
|
||||
NAV_WP_MODE = (1 << 12),
|
||||
} flightModeFlags_e;
|
||||
|
||||
|
|
@ -53,11 +53,12 @@ extern uint16_t flightModeFlags;
|
|||
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
|
||||
|
||||
typedef enum {
|
||||
GPS_FIX_HOME = (1 << 0),
|
||||
GPS_FIX = (1 << 1),
|
||||
CALIBRATE_MAG = (1 << 2),
|
||||
SMALL_ANGLE = (1 << 3),
|
||||
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
|
||||
GPS_FIX_HOME = (1 << 0),
|
||||
GPS_FIX = (1 << 1),
|
||||
CALIBRATE_MAG = (1 << 2),
|
||||
SMALL_ANGLE = (1 << 3),
|
||||
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
|
||||
ANTI_WINDUP = (1 << 5),
|
||||
} stateFlags_t;
|
||||
|
||||
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
|
||||
|
|
|
|||
|
|
@ -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 "config/config.h"
|
||||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "config/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);
|
||||
|
|
@ -61,6 +61,8 @@ uint8_t motorCount;
|
|||
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
bool motorLimitReached = false;
|
||||
|
||||
static mixerConfig_t *mixerConfig;
|
||||
static flight3DConfig_t *flight3DConfig;
|
||||
static escAndServoConfig_t *escAndServoConfig;
|
||||
|
|
|
|||
|
|
@ -191,6 +191,7 @@ void filterServos(void);
|
|||
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
extern bool motorLimitReached;
|
||||
|
||||
struct escAndServoConfig_s;
|
||||
struct rxConfig_s;
|
||||
|
|
|
|||
|
|
@ -2240,33 +2240,6 @@ float getEstimatedActualPosition(int axis)
|
|||
return posControl.actualState.pos.A[axis];
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Interface with PIDs: Angle-Command transformation
|
||||
*-----------------------------------------------------------*/
|
||||
int16_t rcCommandToLeanAngle(int16_t rcCommand)
|
||||
{
|
||||
if (posControl.pidProfile->pidController == PID_CONTROLLER_LUX_FLOAT) {
|
||||
// LuxFloat is the only PID controller that uses raw rcCommand as target angle
|
||||
return rcCommand;
|
||||
}
|
||||
else {
|
||||
// Most PID controllers use 2 * rcCommand as target angle for ANGLE mode
|
||||
return rcCommand * 2;
|
||||
}
|
||||
}
|
||||
|
||||
int16_t leanAngleToRcCommand(int16_t leanAngle)
|
||||
{
|
||||
if (posControl.pidProfile->pidController == PID_CONTROLLER_LUX_FLOAT) {
|
||||
// LuxFloat is the only PID controller that uses raw rcCommand as target angle
|
||||
return leanAngle;
|
||||
}
|
||||
else {
|
||||
// Most PID controllers use 2 * rcCommand as target angle for ANGLE mode
|
||||
return leanAngle / 2;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Ability to execute RTH on external event
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -340,13 +340,13 @@ void applyFixedWingPitchRollThrottleController(void)
|
|||
if (isPitchAndThrottleAdjustmentValid) {
|
||||
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
|
||||
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_dive_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_climb_angle));
|
||||
rcCommand[PITCH] = -leanAngleToRcCommand(pitchCorrection);
|
||||
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection);
|
||||
rcCommand[THROTTLE] = constrain(throttleCorrection, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
|
||||
}
|
||||
|
||||
if (isRollAdjustmentValid) {
|
||||
rollCorrection = constrain(rollCorrection, -DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle), DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle));
|
||||
rcCommand[ROLL] = leanAngleToRcCommand(rollCorrection);
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -440,8 +440,8 @@ static void applyMulticopterPositionController(uint32_t currentTime)
|
|||
}
|
||||
|
||||
if (!bypassPositionController) {
|
||||
rcCommand[PITCH] = leanAngleToRcCommand(posControl.rcAdjustment[PITCH]);
|
||||
rcCommand[ROLL] = leanAngleToRcCommand(posControl.rcAdjustment[ROLL]);
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH]);
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -292,9 +292,6 @@ bool isWaypointMissed(navWaypointPosition_t * waypoint);
|
|||
bool isApproachingLastWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
|
||||
int16_t rcCommandToLeanAngle(int16_t rcCommand);
|
||||
int16_t leanAngleToRcCommand(int16_t leanAngle);
|
||||
|
||||
void updateActualHeading(int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity);
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include <platform.h>
|
||||
|
||||
#include "build_config.h"
|
||||
#include "debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
|
@ -43,12 +44,12 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
#include "flight/gtune.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
extern uint16_t cycleTime;
|
||||
extern uint8_t motorCount;
|
||||
extern bool motorLimitReached;
|
||||
extern float dT;
|
||||
|
||||
int16_t axisPID[3];
|
||||
|
|
@ -60,23 +61,11 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
|||
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
|
||||
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
|
||||
|
||||
static int32_t errorGyroI[3] = { 0, 0, 0 };
|
||||
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig);
|
||||
|
||||
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig); // pid controller function prototype
|
||||
|
||||
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using
|
||||
static float errorGyroIfLimit[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
void pidResetErrorGyro(void)
|
||||
{
|
||||
errorGyroI[ROLL] = 0;
|
||||
errorGyroI[PITCH] = 0;
|
||||
errorGyroI[YAW] = 0;
|
||||
|
||||
errorGyroIf[ROLL] = 0.0f;
|
||||
errorGyroIf[PITCH] = 0.0f;
|
||||
errorGyroIf[YAW] = 0.0f;
|
||||
|
|
@ -85,245 +74,149 @@ void pidResetErrorGyro(void)
|
|||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
static biquad_t deltaBiQuadState[3];
|
||||
static bool deltaStateIsSet = false;
|
||||
static bool deltaFilterInit = false;
|
||||
|
||||
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig)
|
||||
float pidRcCommandToAngle(int16_t stick)
|
||||
{
|
||||
float RateError, errorAngle, AngleRate, gyroRate;
|
||||
float ITerm,PTerm,DTerm;
|
||||
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
|
||||
static float lastError[3];
|
||||
float delta;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
|
||||
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
|
||||
deltaStateIsSet = true;
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
|
||||
// Figure out the raw stick positions
|
||||
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
|
||||
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
|
||||
|
||||
if(ABS(stickPosAil) > ABS(stickPosEle)){
|
||||
mostDeflectedPos = ABS(stickPosAil);
|
||||
}
|
||||
else {
|
||||
mostDeflectedPos = ABS(stickPosEle);
|
||||
}
|
||||
|
||||
// Progressively turn off the horizon self level strength as the stick is banged over
|
||||
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
|
||||
if(pidProfile->H_sensitivity == 0){
|
||||
horizonLevelStrength = 0;
|
||||
} else {
|
||||
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
|
||||
}
|
||||
}
|
||||
|
||||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
uint8_t rate = controlRateConfig->rates[axis];
|
||||
|
||||
if (axis == FD_YAW) {
|
||||
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
|
||||
AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f;
|
||||
} else {
|
||||
// calculate error and limit the angle to the max inclination
|
||||
errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis]) / 10.0f; // 16 bits is ok here
|
||||
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
// it's the ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRate = errorAngle * pidProfile->A_level;
|
||||
} else {
|
||||
//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// mix up angle error to desired AngleRate to add a little auto-level feel
|
||||
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps
|
||||
|
||||
// --------low-level gyro-based PID. ----------
|
||||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
RateError = AngleRate - gyroRate;
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
|
||||
|
||||
// -----calculate I component.
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
|
||||
|
||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||
ITerm = errorGyroIf[axis];
|
||||
|
||||
//-----calculate D-term
|
||||
delta = RateError - lastError[axis];
|
||||
lastError[axis] = RateError;
|
||||
|
||||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
delta = filterApplyBiQuad(delta, &deltaBiQuadState[axis]);
|
||||
}
|
||||
|
||||
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
|
||||
// -----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;
|
||||
axisPID_D[axis] = DTerm;
|
||||
#endif
|
||||
}
|
||||
return stick * 2.0f;
|
||||
}
|
||||
|
||||
static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||
rxConfig_t *rxConfig)
|
||||
int16_t pidAngleToRcCommand(float angleDeciDegrees)
|
||||
{
|
||||
return angleDeciDegrees / 2.0f;
|
||||
}
|
||||
|
||||
float pidRcCommandToRate(int16_t stick, uint8_t rate)
|
||||
{
|
||||
// Map stick position from 200dps to 1200dps
|
||||
return (float)((rate + 20) * stick) / 50.0f;
|
||||
}
|
||||
|
||||
#define FP_PID_RATE_P_MULTIPLIER 40.0f
|
||||
#define FP_PID_RATE_I_MULTIPLIER 40.0f
|
||||
#define FP_PID_RATE_D_MULTIPLIER 4000.0f
|
||||
#define FP_PID_LEVEL_P_MULTIPLIER 40.0f
|
||||
|
||||
void pidController(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
|
||||
int axis;
|
||||
int32_t delta;
|
||||
int32_t PTerm, ITerm, DTerm;
|
||||
static int32_t lastError[3] = { 0, 0, 0 };
|
||||
int32_t AngleRateTmp, RateError, gyroRate;
|
||||
|
||||
int8_t horizonLevelStrength = 100;
|
||||
|
||||
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
|
||||
deltaStateIsSet = true;
|
||||
}
|
||||
float horizonLevelStrength = 1;
|
||||
static float rateErrorBuf[3][5] = { { 0 } };
|
||||
int axis, n;
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// Figure out the raw stick positions
|
||||
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
|
||||
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
|
||||
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
|
||||
|
||||
// Progressively turn off the horizon self level strength as the stick is banged over
|
||||
horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection
|
||||
// Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
|
||||
// For more rate mode increase D and slower flips and rolls will be possible
|
||||
horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
|
||||
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
|
||||
if(pidProfile->D8[PIDLEVEL] == 0){
|
||||
horizonLevelStrength = 0;
|
||||
} else {
|
||||
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100.0f / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
|
||||
}
|
||||
}
|
||||
|
||||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
uint8_t rate = controlRateConfig->rates[axis];
|
||||
float kP = pidProfile->P8[axis] / FP_PID_RATE_P_MULTIPLIER;
|
||||
float kI = pidProfile->I8[axis] / FP_PID_RATE_I_MULTIPLIER;
|
||||
float kD = pidProfile->D8[axis] / FP_PID_RATE_D_MULTIPLIER;
|
||||
bool useIntegralComponent = (pidProfile->P8[axis] != 0) && (pidProfile->I8[axis] != 0);
|
||||
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
|
||||
} else {
|
||||
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
|
||||
float rateTarget; // rotation rate target (dps)
|
||||
|
||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// calculate error and limit the angle to max configured inclination
|
||||
int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
|
||||
// Rate setpoint for ACRO and HORIZON modes
|
||||
rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]);
|
||||
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
// it's the ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
|
||||
} else {
|
||||
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
|
||||
// horizonLevelStrength is scaled to the stick input
|
||||
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
|
||||
}
|
||||
// Outer PID loop (ANGLE/HORIZON)
|
||||
if ((axis != FD_YAW) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
||||
static filterStatePt1_t angleFilterState[2]; // Only ROLL and PITCH
|
||||
|
||||
float angleTarget = pidRcCommandToAngle(rcCommand[axis]);
|
||||
float angleError = (constrain(angleTarget, -pidProfile->max_angle_inclination, +pidProfile->max_angle_inclination) - attitude.raw[axis]) / 10.0f;
|
||||
|
||||
// P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
rateTarget += angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER) * horizonLevelStrength;
|
||||
}
|
||||
else {
|
||||
rateTarget = angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER);
|
||||
}
|
||||
|
||||
// Apply simple LPF to rateTarget to make response less jerky
|
||||
if (pidProfile->I8[PIDLEVEL]) {
|
||||
// I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
|
||||
rateTarget = filterApplyPt1(rateTarget, &angleFilterState[axis], pidProfile->I8[PIDLEVEL], dT);
|
||||
}
|
||||
}
|
||||
|
||||
// --------low-level gyro-based PID. ----------
|
||||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
gyroRate = gyroADC[axis] / 4;
|
||||
RateError = AngleRateTmp - gyroRate;
|
||||
// Limit desired rate to something gyro can measure reliably
|
||||
rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
// Inner PID loop - gyro-based control to reach rateTarget
|
||||
float gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps
|
||||
float rateError = rateTarget - gyroRate;
|
||||
|
||||
// -----calculate I component
|
||||
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
|
||||
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
|
||||
// Time correction (to avoid different I scaling for different builds based on average cycle time)
|
||||
// is normalized to cycle time = 2048.
|
||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)cycleTime) >> 11) * pidProfile->I8[axis];
|
||||
// Calculate new P-term
|
||||
float newPTerm = rateError * kP * (PIDweight[axis] / 100);
|
||||
|
||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
|
||||
ITerm = errorGyroI[axis] >> 13;
|
||||
|
||||
//-----calculate D-term
|
||||
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastError[axis] = RateError;
|
||||
|
||||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)cycleTime >> 4))) >> 6;
|
||||
|
||||
if (deltaStateIsSet) {
|
||||
// Upscale x3 before filtering to avoid rounding errors
|
||||
delta = lrintf(filterApplyBiQuad(3.0f * delta, &deltaBiQuadState[axis]));
|
||||
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
|
||||
newPTerm = constrain(newPTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||
}
|
||||
|
||||
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
// Calculate new D-term
|
||||
// Shift old error values
|
||||
for (n = 4; n > 0; n--) {
|
||||
rateErrorBuf[axis][n] = rateErrorBuf[axis][n-1];
|
||||
}
|
||||
#endif
|
||||
|
||||
// Store new error value
|
||||
rateErrorBuf[axis][0] = rateError;
|
||||
|
||||
// Calculate derivative using 5-point noise-robust differentiator by Pavel Holoborodko
|
||||
float newDTerm = ((2 * (rateErrorBuf[axis][1] - rateErrorBuf[axis][3]) + (rateErrorBuf[axis][0] - rateErrorBuf[axis][4])) / (8 * dT)) * kD * (PIDweight[axis] / 100);
|
||||
|
||||
// Apply additional lowpass
|
||||
if (pidProfile->dterm_lpf_hz) {
|
||||
if (!deltaFilterInit) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
|
||||
deltaFilterInit = true;
|
||||
}
|
||||
|
||||
newDTerm = filterApplyBiQuad(newDTerm, &deltaBiQuadState[axis]);
|
||||
}
|
||||
|
||||
// TODO: Get feedback from mixer on available correction range for each axis
|
||||
float newOutput = newPTerm + errorGyroIf[axis] + newDTerm;
|
||||
float newOutputLimited = constrainf(newOutput, -PID_MAX_OUTPUT, +PID_MAX_OUTPUT);
|
||||
|
||||
// Integrate only if we can do backtracking
|
||||
if (useIntegralComponent) {
|
||||
float kT = 2.0f / ((kP / kI) + (kD / kP));
|
||||
errorGyroIf[axis] += (rateError * kI * dT) + ((newOutputLimited - newOutput) * kT * dT);
|
||||
|
||||
// Don't grow I-term if motors are at their limit
|
||||
if (STATE(ANTI_WINDUP) || motorLimitReached) {
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
|
||||
} else {
|
||||
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
|
||||
}
|
||||
} else {
|
||||
errorGyroIf[axis] = 0;
|
||||
}
|
||||
|
||||
axisPID[axis] = newOutputLimited;
|
||||
|
||||
debug[axis] = newOutputLimited;
|
||||
|
||||
#ifdef BLACKBOX
|
||||
axisPID_P[axis] = PTerm;
|
||||
axisPID_I[axis] = ITerm;
|
||||
axisPID_D[axis] = DTerm;
|
||||
axisPID_P[axis] = newPTerm;
|
||||
axisPID_I[axis] = errorGyroIf[axis];
|
||||
axisPID_D[axis] = newDTerm;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void pidSetController(pidControllerType_e type)
|
||||
{
|
||||
// Force filter re-init
|
||||
deltaStateIsSet = false;
|
||||
|
||||
switch (type) {
|
||||
default:
|
||||
case PID_CONTROLLER_MWREWRITE:
|
||||
pid_controller = pidMultiWiiRewrite;
|
||||
break;
|
||||
case PID_CONTROLLER_LUX_FLOAT:
|
||||
pid_controller = pidLuxFloat;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,9 +17,16 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define GYRO_I_MAX 256 // Gyro I limiter
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
#define GYRO_SATURATION_LIMIT 1800 // 1800dps
|
||||
#define PID_MAX_OUTPUT 1000
|
||||
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
|
||||
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
||||
#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter
|
||||
|
||||
typedef enum {
|
||||
PIDROLL,
|
||||
|
|
@ -35,46 +42,27 @@ typedef enum {
|
|||
PID_ITEM_COUNT
|
||||
} pidIndex_e;
|
||||
|
||||
typedef enum {
|
||||
PID_CONTROLLER_MWREWRITE = 1,
|
||||
PID_CONTROLLER_LUX_FLOAT,
|
||||
PID_COUNT
|
||||
} pidControllerType_e;
|
||||
|
||||
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
|
||||
|
||||
typedef struct pidProfile_s {
|
||||
uint8_t pidController; // 1 = rewrite, 2 = luxfloat
|
||||
|
||||
uint8_t P8[PID_ITEM_COUNT];
|
||||
uint8_t I8[PID_ITEM_COUNT];
|
||||
uint8_t D8[PID_ITEM_COUNT];
|
||||
|
||||
float P_f[3]; // float p i and d factors for lux float pid controller
|
||||
float I_f[3];
|
||||
float D_f[3];
|
||||
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 dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
|
||||
|
||||
uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames
|
||||
uint8_t gyro_soft_lpf_hz; // Gyro FIR filtering
|
||||
uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
|
||||
|
||||
#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
|
||||
uint16_t yaw_p_limit;
|
||||
|
||||
int16_t max_angle_inclination; // Max possible inclination
|
||||
} pidProfile_t;
|
||||
|
||||
extern int16_t axisPID[XYZ_AXIS_COUNT];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
|
||||
void pidSetController(pidControllerType_e type);
|
||||
void pidResetErrorGyro(void);
|
||||
|
||||
float pidRcCommandToAngle(int16_t stick);
|
||||
int16_t pidAngleToRcCommand(float angleDeciDegrees);
|
||||
|
||||
void pidController(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@
|
|||
|
||||
#include "mw.h"
|
||||
|
||||
#define AIRMODE_DEADBAND 10
|
||||
|
||||
static escAndServoConfig_t *escAndServoConfig;
|
||||
static pidProfile_t *pidProfile;
|
||||
|
|
@ -133,6 +134,15 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
|
|||
return THROTTLE_HIGH;
|
||||
}
|
||||
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
|
||||
{
|
||||
if (((rcData[PITCH] < (rxConfig->midrc + AIRMODE_DEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODE_DEADBAND)))
|
||||
&& ((rcData[ROLL] < (rxConfig->midrc + AIRMODE_DEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODE_DEADBAND))))
|
||||
return CENTERED;
|
||||
|
||||
return NOT_CENTERED;
|
||||
}
|
||||
|
||||
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
|
||||
{
|
||||
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
||||
|
|
@ -439,7 +449,6 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
|
|||
|
||||
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
|
||||
int newValue;
|
||||
float newFloatValue;
|
||||
|
||||
if (delta > 0) {
|
||||
beeperConfirmationBeeps(2);
|
||||
|
|
@ -486,114 +495,60 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
|
|||
break;
|
||||
case ADJUSTMENT_PITCH_ROLL_P:
|
||||
case ADJUSTMENT_PITCH_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDPITCH] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
|
||||
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
|
||||
case ADJUSTMENT_ROLL_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDROLL] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_PITCH_ROLL_I:
|
||||
case ADJUSTMENT_PITCH_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDPITCH] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
|
||||
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
|
||||
case ADJUSTMENT_ROLL_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDROLL] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_PITCH_ROLL_D:
|
||||
case ADJUSTMENT_PITCH_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D_f[PIDPITCH] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDPITCH] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
|
||||
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
|
||||
break;
|
||||
}
|
||||
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
|
||||
case ADJUSTMENT_ROLL_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D_f[PIDROLL] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDROLL] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_YAW_P:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P_f[PIDYAW] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->P8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_YAW_I:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I_f[PIDYAW] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->I8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
|
||||
break;
|
||||
case ADJUSTMENT_YAW_D:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
|
||||
newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D_f[PIDYAW] = newFloatValue;
|
||||
blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue);
|
||||
} else {
|
||||
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
|
||||
}
|
||||
newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
|
||||
pidProfile->D8[PIDYAW] = newValue;
|
||||
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ typedef enum {
|
|||
BOXGOV,
|
||||
BOXOSD,
|
||||
BOXTELEMETRY,
|
||||
BOXGTUNE,
|
||||
//BOXGTUNE,
|
||||
BOXSERVO1,
|
||||
BOXSERVO2,
|
||||
BOXSERVO3,
|
||||
|
|
@ -77,6 +77,11 @@ typedef enum {
|
|||
THROTTLE_HIGH
|
||||
} throttleStatus_e;
|
||||
|
||||
typedef enum {
|
||||
NOT_CENTERED = 0,
|
||||
CENTERED
|
||||
} rollPitchStatus_e;
|
||||
|
||||
#define ROL_LO (1 << (2 * ROLL))
|
||||
#define ROL_CE (3 << (2 * ROLL))
|
||||
#define ROL_HI (2 << (2 * ROLL))
|
||||
|
|
@ -126,8 +131,6 @@ typedef struct modeActivationCondition_s {
|
|||
|
||||
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
|
||||
|
||||
#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || ((throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))))
|
||||
|
||||
typedef struct controlRateConfig_s {
|
||||
uint8_t rcRate8;
|
||||
uint8_t rcExpo8;
|
||||
|
|
@ -152,6 +155,7 @@ bool areUsingSticksToArm(void);
|
|||
|
||||
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig);
|
||||
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
|
||||
|
||||
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
|
||||
|
|
|
|||
|
|
@ -645,8 +645,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDeciDegrees, .config.minmax = { -1800, 3600 }, 0 },
|
||||
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDeciDegrees, .config.minmax = { -1800, 3600 }, 0 },
|
||||
|
||||
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 }, 0 },
|
||||
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 }, 0 },
|
||||
|
||||
|
|
@ -666,6 +664,7 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 }, 0 },
|
||||
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 },
|
||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, 0 },
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 },
|
||||
|
|
@ -707,8 +706,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, .config.minmax = { 0, MAG_MAX }, 0 },
|
||||
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 }, 0 },
|
||||
|
||||
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER }, 0 },
|
||||
|
||||
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 }, 0 },
|
||||
|
|
@ -719,41 +716,16 @@ const clivalue_t valueTable[] = {
|
|||
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 }, 0 },
|
||||
|
||||
{ "p_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[PITCH], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "i_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[PITCH], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "d_pitchf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[PITCH], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "p_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[ROLL], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "i_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[ROLL], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "d_rollf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[ROLL], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "p_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P_f[YAW], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "i_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I_f[YAW], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "d_yawf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D_f[YAW], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], .config.minmax = { 0, 255 }, 0 },
|
||||
{ "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 },
|
||||
{ "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 },
|
||||
|
||||
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, .config.minmax = { 0, 10 }, 0 },
|
||||
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, .config.minmax = { 0, 10 }, 0 },
|
||||
{ "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, .config.minmax = { 0, 250 }, 0 },
|
||||
{ "max_angle_inclination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination, .config.minmax = { 100, 900 }, 0 },
|
||||
|
||||
{ "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 },
|
||||
|
||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
||||
{ "gyro_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } },
|
||||
{ "acc_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },
|
||||
{ "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
|
||||
|
||||
#ifdef GTUNE
|
||||
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 }, 0 },
|
||||
{ "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 }, 0 },
|
||||
{ "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 }, 0 },
|
||||
{ "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 }, 0 },
|
||||
{ "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 }, 0 },
|
||||
{ "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 }, 0 },
|
||||
{ "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 }, 0 },
|
||||
#endif
|
||||
|
||||
#ifdef BLACKBOX
|
||||
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 }, 0 },
|
||||
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 }, 0 },
|
||||
|
|
|
|||
|
|
@ -358,7 +358,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ BOXGOV, "GOVERNOR;", 18 },
|
||||
{ BOXOSD, "OSD SW;", 19 },
|
||||
{ BOXTELEMETRY, "TELEMETRY;", 20 },
|
||||
{ BOXGTUNE, "GTUNE;", 21 },
|
||||
//{ BOXGTUNE, "GTUNE;", 21 },
|
||||
{ BOXSERVO1, "SERVO1;", 23 },
|
||||
{ BOXSERVO2, "SERVO2;", 24 },
|
||||
{ BOXSERVO3, "SERVO3;", 25 },
|
||||
|
|
@ -715,10 +715,6 @@ void mspInit(serialConfig_t *serialConfig)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
|
||||
#endif
|
||||
|
||||
memset(mspPorts, 0x00, sizeof(mspPorts));
|
||||
mspAllocateSerialPorts(serialConfig);
|
||||
}
|
||||
|
|
@ -748,7 +744,6 @@ static uint32_t packFlightModeFlags(void)
|
|||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
|
||||
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
|
||||
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
|
||||
|
|
@ -994,29 +989,10 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_PID:
|
||||
headSerialReply(3 * PID_ITEM_COUNT);
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
|
||||
for (i = 0; i < 3; i++) {
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 255));
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 255));
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 255));
|
||||
}
|
||||
for (i = 3; i < PID_ITEM_COUNT; i++) {
|
||||
if (i == PIDLEVEL) {
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 255));
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 255));
|
||||
serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 255));
|
||||
} else {
|
||||
serialize8(currentProfile->pidProfile.P8[i]);
|
||||
serialize8(currentProfile->pidProfile.I8[i]);
|
||||
serialize8(currentProfile->pidProfile.D8[i]);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
serialize8(currentProfile->pidProfile.P8[i]);
|
||||
serialize8(currentProfile->pidProfile.I8[i]);
|
||||
serialize8(currentProfile->pidProfile.D8[i]);
|
||||
}
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
serialize8(currentProfile->pidProfile.P8[i]);
|
||||
serialize8(currentProfile->pidProfile.I8[i]);
|
||||
serialize8(currentProfile->pidProfile.D8[i]);
|
||||
}
|
||||
break;
|
||||
case MSP_PIDNAMES:
|
||||
|
|
@ -1025,7 +1001,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_PID_CONTROLLER:
|
||||
headSerialReply(1);
|
||||
serialize8(currentProfile->pidProfile.pidController);
|
||||
serialize8(2); // FIXME: Report as LuxFloat
|
||||
break;
|
||||
case MSP_MODE_RANGES:
|
||||
headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
|
||||
|
|
@ -1430,33 +1406,13 @@ static bool processInCommand(void)
|
|||
masterConfig.looptime = read16();
|
||||
break;
|
||||
case MSP_SET_PID_CONTROLLER:
|
||||
currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility
|
||||
pidSetController(currentProfile->pidProfile.pidController);
|
||||
// FIXME: Do nothing
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
|
||||
for (i = 0; i < 3; i++) {
|
||||
currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
|
||||
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
|
||||
currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f;
|
||||
}
|
||||
for (i = 3; i < PID_ITEM_COUNT; i++) {
|
||||
if (i == PIDLEVEL) {
|
||||
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
|
||||
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
|
||||
currentProfile->pidProfile.H_sensitivity = read8();
|
||||
} else {
|
||||
currentProfile->pidProfile.P8[i] = read8();
|
||||
currentProfile->pidProfile.I8[i] = read8();
|
||||
currentProfile->pidProfile.D8[i] = read8();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
currentProfile->pidProfile.P8[i] = read8();
|
||||
currentProfile->pidProfile.I8[i] = read8();
|
||||
currentProfile->pidProfile.D8[i] = read8();
|
||||
}
|
||||
for (i = 0; i < PID_ITEM_COUNT; i++) {
|
||||
currentProfile->pidProfile.P8[i] = read8();
|
||||
currentProfile->pidProfile.I8[i] = read8();
|
||||
currentProfile->pidProfile.D8[i] = read8();
|
||||
}
|
||||
break;
|
||||
case MSP_SET_MODE_RANGE:
|
||||
|
|
|
|||
|
|
@ -76,7 +76,6 @@
|
|||
#include "flight/imu.h"
|
||||
#include "flight/hil.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/gtune.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
|
@ -115,35 +114,6 @@ extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
|
|||
|
||||
static bool isRXDataNew;
|
||||
|
||||
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig); // pid controller function prototype
|
||||
|
||||
extern pidControllerFuncPtr pid_controller;
|
||||
|
||||
#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
|
||||
|
|
@ -401,8 +371,26 @@ void processRx(void)
|
|||
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
|
||||
if (SHOULD_RESET_ERRORS) {
|
||||
pidResetErrorGyro();
|
||||
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
|
||||
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
|
||||
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
|
||||
if (throttleStatus == THROTTLE_LOW) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
|
||||
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
|
||||
|
||||
if (rollPitchStatus == CENTERED) {
|
||||
ENABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
pidResetErrorGyro();
|
||||
}
|
||||
}
|
||||
else {
|
||||
DISABLE_STATE(ANTI_WINDUP);
|
||||
}
|
||||
|
||||
// When armed and motors aren't spinning, do beeps and then disarm
|
||||
|
|
@ -604,10 +592,6 @@ void taskMainPidLoop(void)
|
|||
|
||||
isRXDataNew = false;
|
||||
|
||||
#ifdef GTUNE
|
||||
updateGtuneState();
|
||||
#endif
|
||||
|
||||
#if defined(NAV)
|
||||
updatePositionEstimator();
|
||||
applyWaypointNavigationAndAltitudeHold();
|
||||
|
|
@ -655,13 +639,7 @@ void taskMainPidLoop(void)
|
|||
}
|
||||
}
|
||||
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(
|
||||
¤tProfile->pidProfile,
|
||||
currentControlRateProfile,
|
||||
masterConfig.max_angle_inclination,
|
||||
&masterConfig.rxConfig
|
||||
);
|
||||
pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
|
||||
|
||||
#ifdef HIL
|
||||
if (hilActive) {
|
||||
|
|
|
|||
|
|
@ -118,7 +118,6 @@
|
|||
//#define GPS_PROTO_I2C_NAV
|
||||
//#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
//#define DISPLAY
|
||||
//#define DISPLAY_ARMED_BITMAP
|
||||
#define USE_SERVOS
|
||||
|
|
|
|||
|
|
@ -124,7 +124,6 @@
|
|||
#endif
|
||||
|
||||
#define BLACKBOX
|
||||
#define GTUNE
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_FRSKY
|
||||
#define TELEMETRY_HOTT
|
||||
|
|
|
|||
|
|
@ -78,7 +78,6 @@
|
|||
#endif
|
||||
|
||||
//#undef USE_CLI
|
||||
#define GTUNE
|
||||
//#define BLACKBOX
|
||||
|
||||
#define DISABLE_UNCOMMON_MIXERS
|
||||
|
|
|
|||
|
|
@ -150,7 +150,6 @@
|
|||
#define GPS_PROTO_I2C_NAV
|
||||
#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
#define LED_STRIP
|
||||
|
||||
#define LED_STRIP_TIMER TIM16
|
||||
|
|
|
|||
|
|
@ -128,7 +128,6 @@
|
|||
#define LED_STRIP_TIMER TIM3
|
||||
|
||||
#define BLACKBOX
|
||||
#define GTUNE
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_FRSKY
|
||||
#define TELEMETRY_HOTT
|
||||
|
|
|
|||
|
|
@ -180,8 +180,6 @@
|
|||
|
||||
#define NAV
|
||||
|
||||
//#define GTUNE
|
||||
|
||||
//#define LED_STRIP
|
||||
//#define LED_STRIP_TIMER TIM3
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@
|
|||
#define GPS_PROTO_I2C_NAV
|
||||
#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
#define SERIAL_RX
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_FRSKY
|
||||
|
|
|
|||
|
|
@ -157,7 +157,6 @@
|
|||
#define GPS_PROTO_I2C_NAV
|
||||
#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
#define SERIAL_RX
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_FRSKY
|
||||
|
|
|
|||
|
|
@ -118,8 +118,6 @@
|
|||
#define GPS_PROTO_I2C_NAV
|
||||
#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
|
||||
#define DISPLAY
|
||||
#define DISPLAY_ARMED_BITMAP
|
||||
|
||||
|
|
|
|||
|
|
@ -174,7 +174,6 @@
|
|||
|
||||
#define NAV
|
||||
|
||||
#define GTUNE
|
||||
#define SERIAL_RX
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_FRSKY
|
||||
|
|
|
|||
|
|
@ -97,7 +97,6 @@
|
|||
#define GPS_PROTO_I2C_NAV
|
||||
#define GPS_PROTO_NAZA
|
||||
|
||||
#define GTUNE
|
||||
#define LED_STRIP
|
||||
#define LED_STRIP_TIMER TIM16
|
||||
#define TELEMETRY
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue