2.8.1 changes ready for merge
This commit is contained in:
blckmn
parent
6927139261
commit
897aab9c86
|
|
@ -180,29 +180,34 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
|||
|
||||
static void resetPidProfile(pidProfile_t *pidProfile)
|
||||
{
|
||||
pidProfile->pidController = 1;
|
||||
|
||||
#if (defined(STM32F10X))
|
||||
pidProfile->pidController = PID_CONTROLLER_MWREWRITE;
|
||||
#else
|
||||
pidProfile->pidController = PID_CONTROLLER_LUX_FLOAT;
|
||||
#endif
|
||||
|
||||
pidProfile->P8[ROLL] = 45;
|
||||
pidProfile->I8[ROLL] = 40;
|
||||
pidProfile->D8[ROLL] = 15;
|
||||
pidProfile->P8[PITCH] = 45;
|
||||
pidProfile->D8[ROLL] = 18;
|
||||
pidProfile->P8[PITCH] = 50;
|
||||
pidProfile->I8[PITCH] = 40;
|
||||
pidProfile->D8[PITCH] = 15;
|
||||
pidProfile->D8[PITCH] = 18;
|
||||
pidProfile->P8[YAW] = 90;
|
||||
pidProfile->I8[YAW] = 45;
|
||||
pidProfile->D8[YAW] = 20;
|
||||
pidProfile->P8[PIDALT] = 50;
|
||||
pidProfile->I8[PIDALT] = 0;
|
||||
pidProfile->D8[PIDALT] = 0;
|
||||
pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
|
||||
pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
|
||||
pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
|
||||
pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
|
||||
pidProfile->D8[PIDPOS] = 0;
|
||||
pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
|
||||
pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
|
||||
pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
|
||||
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
|
||||
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
|
||||
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
|
||||
pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
|
||||
pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
|
||||
pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
|
||||
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
|
||||
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
|
||||
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
|
||||
pidProfile->P8[PIDLEVEL] = 50;
|
||||
pidProfile->I8[PIDLEVEL] = 50;
|
||||
pidProfile->D8[PIDLEVEL] = 100;
|
||||
|
|
@ -214,8 +219,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->yaw_lpf_hz = 80;
|
||||
pidProfile->rollPitchItermIgnoreRate = 200;
|
||||
pidProfile->yawItermIgnoreRate = 45;
|
||||
pidProfile->dterm_lpf_hz = 110; // filtering ON by default
|
||||
pidProfile->yawItermIgnoreRate = 35;
|
||||
pidProfile->dterm_lpf_hz = 50; // filtering ON by default
|
||||
pidProfile->dynamic_pid = 1;
|
||||
|
||||
#ifdef GTUNE
|
||||
|
|
@ -265,7 +270,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
|
|||
escAndServoConfig->maxthrottle = 1850;
|
||||
escAndServoConfig->mincommand = 1000;
|
||||
escAndServoConfig->servoCenterPulse = 1500;
|
||||
escAndServoConfig->escDesyncProtection = 10000;
|
||||
escAndServoConfig->escDesyncProtection = 0;
|
||||
}
|
||||
|
||||
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
|
||||
|
|
@ -388,14 +393,14 @@ uint8_t getCurrentControlRateProfile(void)
|
|||
return currentControlRateProfileIndex;
|
||||
}
|
||||
|
||||
static void setControlRateProfile(uint8_t profileIndex)
|
||||
{
|
||||
currentControlRateProfileIndex = profileIndex;
|
||||
masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex;
|
||||
currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex];
|
||||
static void setControlRateProfile(uint8_t profileIndex)
|
||||
{
|
||||
currentControlRateProfileIndex = profileIndex;
|
||||
masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex;
|
||||
currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex];
|
||||
}
|
||||
|
||||
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
|
||||
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
|
||||
{
|
||||
return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile];
|
||||
}
|
||||
|
|
@ -489,10 +494,10 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.rssi_channel = 0;
|
||||
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
|
||||
masterConfig.rxConfig.rssi_ppm_invert = 0;
|
||||
masterConfig.rxConfig.rcSmoothing = 0;
|
||||
masterConfig.rxConfig.rcSmoothing = 0; // TODO - Cleanup with next EEPROM changes
|
||||
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
|
||||
#ifdef STM32F4
|
||||
masterConfig.rxConfig.max_aux_channel = 99;
|
||||
masterConfig.rxConfig.max_aux_channel = 99;
|
||||
#else
|
||||
masterConfig.rxConfig.max_aux_channel = 6;
|
||||
#endif
|
||||
|
|
@ -541,11 +546,11 @@ static void resetConf(void)
|
|||
masterConfig.emf_avoidance = 0; // TODO - needs removal
|
||||
|
||||
resetPidProfile(¤tProfile->pidProfile);
|
||||
|
||||
uint8_t rI;
|
||||
for (rI = 0; rI<MAX_RATEPROFILES; rI++) {
|
||||
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]);
|
||||
}
|
||||
|
||||
uint8_t rI;
|
||||
for (rI = 0; rI<MAX_RATEPROFILES; rI++) {
|
||||
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]);
|
||||
}
|
||||
resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
|
||||
|
||||
masterConfig.mag_declination = 0;
|
||||
|
|
@ -887,22 +892,22 @@ void validateAndFixConfig(void)
|
|||
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
|
||||
// shared pin
|
||||
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
|
||||
featureClear(FEATURE_SONAR);
|
||||
featureClear(FEATURE_SOFTSERIAL);
|
||||
featureClear(FEATURE_RSSI_ADC);
|
||||
featureClear(FEATURE_SONAR);
|
||||
featureClear(FEATURE_SOFTSERIAL);
|
||||
featureClear(FEATURE_RSSI_ADC);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(COLIBRI_RACE)
|
||||
masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
|
||||
if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
|
||||
featureClear(FEATURE_RX_PARALLEL_PWM);
|
||||
featureClear(FEATURE_RX_MSP);
|
||||
featureSet(FEATURE_RX_PPM);
|
||||
featureClear(FEATURE_RX_PARALLEL_PWM);
|
||||
featureClear(FEATURE_RX_MSP);
|
||||
featureSet(FEATURE_RX_PPM);
|
||||
}
|
||||
if(featureConfigured(FEATURE_RX_SERIAL)) {
|
||||
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
//masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
|
||||
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
//masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -980,11 +985,11 @@ void writeEEPROM(void)
|
|||
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
|
||||
if (wordOffset % FLASH_PAGE_SIZE == 0) {
|
||||
#if defined(STM32F40_41xxx)
|
||||
status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
|
||||
status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
|
||||
#elif defined (STM32F411xE)
|
||||
status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
|
||||
status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
|
||||
#else
|
||||
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
|
||||
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
|
||||
#endif
|
||||
if (status != FLASH_COMPLETE) {
|
||||
break;
|
||||
|
|
@ -1040,12 +1045,12 @@ void changeProfile(uint8_t profileIndex)
|
|||
beeperConfirmationBeeps(profileIndex + 1);
|
||||
}
|
||||
|
||||
void changeControlRateProfile(uint8_t profileIndex) {
|
||||
if (profileIndex > MAX_RATEPROFILES) {
|
||||
profileIndex = MAX_RATEPROFILES - 1;
|
||||
}
|
||||
setControlRateProfile(profileIndex);
|
||||
activateControlRateConfig();
|
||||
void changeControlRateProfile(uint8_t profileIndex) {
|
||||
if (profileIndex > MAX_RATEPROFILES) {
|
||||
profileIndex = MAX_RATEPROFILES - 1;
|
||||
}
|
||||
setControlRateProfile(profileIndex);
|
||||
activateControlRateConfig();
|
||||
}
|
||||
|
||||
void handleOneshotFeatureChangeOnRestart(void)
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
#define PWM_TIMER_MHZ 1
|
||||
|
||||
#define PWM_BRUSHED_TIMER_MHZ 8
|
||||
#define MULTISHOT_TIMER_MHZ 12
|
||||
#define MULTISHOT_TIMER_MHZ 72
|
||||
#define ONESHOT42_TIMER_MHZ 24
|
||||
#define ONESHOT125_TIMER_MHZ 8
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -135,7 +136,7 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
|
|||
|
||||
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
|
||||
{
|
||||
*motors[index]->ccr = 60001 * (value - 1000) / 250000 + 60;
|
||||
*motors[index]->ccr = lrintf(((float)(value-1000) / 0.69444f) + 360);
|
||||
}
|
||||
|
||||
void pwmWriteMotor(uint8_t index, uint16_t value)
|
||||
|
|
|
|||
|
|
@ -70,6 +70,8 @@ static rxConfig_t *rxConfig;
|
|||
static mixerMode_e currentMixerMode;
|
||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
float errorLimiter = 1.0f;
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
static uint8_t servoRuleCount = 0;
|
||||
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
|
||||
|
|
@ -823,22 +825,22 @@ void mixTable(void)
|
|||
throttleRange = throttleMax - throttleMin;
|
||||
|
||||
if (rollPitchYawMixRange > throttleRange) {
|
||||
motorLimitReached = true;
|
||||
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
|
||||
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
|
||||
}
|
||||
// Get the maximum correction by setting offset to center
|
||||
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
|
||||
|
||||
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
|
||||
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
|
||||
} else {
|
||||
motorLimitReached = false;
|
||||
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
|
||||
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
|
||||
}
|
||||
|
||||
// adjust feedback to scale PID error inputs to our limitations.
|
||||
errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f);
|
||||
if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100;
|
||||
|
||||
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
|
||||
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
|
|
|
|||
|
|
@ -189,7 +189,6 @@ void filterServos(void);
|
|||
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||
bool motorLimitReached;
|
||||
struct escAndServoConfig_s;
|
||||
struct rxConfig_s;
|
||||
|
||||
|
|
|
|||
|
|
@ -49,8 +49,9 @@
|
|||
#include "config/runtime_config.h"
|
||||
|
||||
extern uint8_t motorCount;
|
||||
extern bool motorLimitReached;
|
||||
uint32_t targetPidLooptime;
|
||||
extern float errorLimiter;
|
||||
extern float angleRate[3], angleRateSmooth[2];
|
||||
|
||||
int16_t axisPID[3];
|
||||
|
||||
|
|
@ -61,13 +62,13 @@ 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 PIDweight[3];
|
||||
|
||||
static int32_t errorGyroI[3], errorGyroILimit[3];
|
||||
static int32_t errorGyroI[3];
|
||||
#ifndef SKIP_PID_LUXFLOAT
|
||||
static float errorGyroIf[3], errorGyroIfLimit[3];
|
||||
static float errorGyroIf[3];
|
||||
#endif
|
||||
|
||||
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
|
||||
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
|
||||
|
||||
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
|
||||
|
||||
|
|
@ -75,38 +76,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
|
|||
targetPidLooptime = targetLooptime * pidProcessDenom;
|
||||
}
|
||||
|
||||
float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) {
|
||||
float angleRate;
|
||||
|
||||
if (isSuperExpoActive()) {
|
||||
float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcYawRate8 / 100.0f))) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
|
||||
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f));
|
||||
|
||||
angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f);
|
||||
} else {
|
||||
angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f;
|
||||
}
|
||||
|
||||
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
|
||||
}
|
||||
|
||||
uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
|
||||
uint16_t dynamicKp;
|
||||
|
||||
uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);
|
||||
|
||||
dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;
|
||||
|
||||
return dynamicKp;
|
||||
}
|
||||
|
||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
|
||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) {
|
||||
uint16_t dynamicKi;
|
||||
uint16_t resetRate;
|
||||
|
||||
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
|
||||
uint32_t dynamicFactor = ((resetRate << 16) / (resetRate + ABS(gyroADC[axis]))) >> 8;
|
||||
uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
|
||||
|
||||
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
|
||||
|
||||
return dynamicKi;
|
||||
|
|
@ -137,12 +114,12 @@ static filterStatePt1_t deltaFilterState[3];
|
|||
static filterStatePt1_t yawFilterState;
|
||||
|
||||
#ifndef SKIP_PID_LUXFLOAT
|
||||
static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
|
||||
static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
|
||||
{
|
||||
float RateError, AngleRate, gyroRate;
|
||||
float RateError, gyroRate, RateErrorSmooth;
|
||||
float ITerm,PTerm,DTerm;
|
||||
static float lastRate[3];
|
||||
static float lastRateError[2];
|
||||
float delta;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
|
|
@ -171,40 +148,43 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
|
||||
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
|
||||
AngleRate = calculateRate(axis, controlRateConfig);
|
||||
|
||||
// Yaw control is GYRO based, direct sticks control is applied to rate PID
|
||||
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
|
||||
// calculate error angle and limit the angle to the max inclination
|
||||
#ifdef GPS
|
||||
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
|
||||
const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
|
||||
#else
|
||||
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
|
||||
const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
|
||||
#endif
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
// ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
|
||||
angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
|
||||
} else {
|
||||
// HORIZON mode - direct sticks control is applied to rate PID
|
||||
// mix up angle error to desired AngleRate to add a little auto-level feel
|
||||
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
|
||||
angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f);
|
||||
}
|
||||
}
|
||||
|
||||
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale
|
||||
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to old rewrite scale
|
||||
|
||||
// --------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;
|
||||
RateError = (angleRate[axis] - gyroRate) * errorLimiter;
|
||||
|
||||
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
// Smoothed Error for Derivative
|
||||
if (flightModeFlags && axis != YAW) {
|
||||
RateErrorSmooth = RateError;
|
||||
} else {
|
||||
RateErrorSmooth = (angleRateSmooth[axis] - gyroRate) * errorLimiter;
|
||||
}
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = luxPTermScale * RateError * kP * tpaFactor;
|
||||
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
|
||||
|
||||
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
|
||||
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
|
||||
|
|
@ -212,16 +192,10 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
}
|
||||
|
||||
// -----calculate I component.
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
|
||||
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
|
||||
|
||||
if (motorLimitReached) {
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
|
||||
} else {
|
||||
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
|
||||
}
|
||||
|
||||
// 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];
|
||||
|
|
@ -241,8 +215,8 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
|
||||
DTerm = 0.0f; // needed for blackbox
|
||||
} else {
|
||||
delta = -(gyroRate - lastRate[axis]);
|
||||
lastRate[axis] = gyroRate;
|
||||
delta = RateErrorSmooth - lastRateError[axis];
|
||||
lastRateError[axis] = RateErrorSmooth;
|
||||
|
||||
// Divide delta by targetLooptime to get differential (ie dr/dt)
|
||||
delta *= (1.0f / getdT());
|
||||
|
|
@ -271,13 +245,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
}
|
||||
#endif
|
||||
|
||||
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
|
||||
{
|
||||
int axis;
|
||||
int32_t PTerm, ITerm, DTerm, delta;
|
||||
static int32_t lastRate[3];
|
||||
int32_t AngleRateTmp, RateError, gyroRate;
|
||||
static int32_t lastRateError[3];
|
||||
int32_t AngleRateTmp, AngleRateTmpSmooth, RateError, gyroRate, RateErrorSmooth;
|
||||
|
||||
int8_t horizonLevelStrength = 100;
|
||||
|
||||
|
|
@ -297,15 +271,16 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
for (axis = 0; axis < 3; axis++) {
|
||||
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig);
|
||||
AngleRateTmp = (int32_t)angleRate[axis];
|
||||
if (axis != YAW) AngleRateTmpSmooth = (int32_t)angleRateSmooth[axis];
|
||||
|
||||
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
|
||||
// calculate error angle and limit the angle to max configured inclination
|
||||
#ifdef GPS
|
||||
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
|
||||
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
|
||||
#else
|
||||
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
|
||||
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
|
||||
#endif
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
|
|
@ -314,7 +289,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
} 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;
|
||||
AngleRateTmp = AngleRateTmpSmooth + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -323,12 +298,18 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
gyroRate = gyroADC[axis] / 4;
|
||||
RateError = AngleRateTmp - gyroRate;
|
||||
|
||||
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
RateError = (AngleRateTmp - gyroRate) * errorLimiter;
|
||||
|
||||
// Smoothed Error for Derivative
|
||||
if (flightModeFlags && axis != YAW) {
|
||||
RateErrorSmooth = RateError;
|
||||
} else {
|
||||
RateErrorSmooth = (AngleRateTmpSmooth - gyroRate) * errorLimiter;
|
||||
}
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
|
||||
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
|
||||
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
|
||||
|
|
@ -340,7 +321,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
// 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.
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
|
||||
|
||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
|
||||
|
||||
|
|
@ -348,12 +329,6 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
// 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);
|
||||
|
||||
if (motorLimitReached) {
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
|
||||
} else {
|
||||
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
|
||||
}
|
||||
|
||||
ITerm = errorGyroI[axis] >> 13;
|
||||
|
||||
//-----calculate D-term
|
||||
|
|
@ -371,8 +346,8 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
|
||||
DTerm = 0; // needed for blackbox
|
||||
} else {
|
||||
delta = -(gyroRate - lastRate[axis]);
|
||||
lastRate[axis] = gyroRate;
|
||||
delta = RateErrorSmooth - lastRateError[axis];
|
||||
lastRateError[axis] = RateErrorSmooth;
|
||||
|
||||
// Divide delta by targetLooptime to get differential (ie dr/dt)
|
||||
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
|
||||
|
|
|
|||
|
|
@ -86,8 +86,8 @@ typedef struct pidProfile_s {
|
|||
struct controlRateConfig_s;
|
||||
union rollAndPitchTrims_u;
|
||||
struct rxConfig_s;
|
||||
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig,
|
||||
uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
|
||||
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
|
||||
|
||||
extern int16_t axisPID[XYZ_AXIS_COUNT];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ static pidProfile_t *pidProfile;
|
|||
static bool isUsingSticksToArm = true;
|
||||
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
int16_t rcCommandSmooth[4];
|
||||
|
||||
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
|
||||
|
||||
|
|
|
|||
|
|
@ -147,6 +147,7 @@ typedef struct controlRateConfig_s {
|
|||
} controlRateConfig_t;
|
||||
|
||||
extern int16_t rcCommand[4];
|
||||
extern int16_t rcCommandSmooth[4];
|
||||
|
||||
typedef struct rcControlsConfig_s {
|
||||
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
|
||||
|
|
|
|||
|
|
@ -584,7 +584,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
|
||||
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
|
||||
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } },
|
||||
{ "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
|
||||
|
|
@ -755,9 +754,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
|
||||
{ "dynamic_pid", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {50, 1000 } },
|
||||
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {25, 1000 } },
|
||||
{ "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
|
||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
|
||||
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
|
||||
|
||||
|
|
|
|||
|
|
@ -1257,6 +1257,29 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(masterConfig.sensorAlignmentConfig.acc_align);
|
||||
serialize8(masterConfig.sensorAlignmentConfig.mag_align);
|
||||
break;
|
||||
case MSP_PID_ADVANCED_CONFIG :
|
||||
headSerialReply(5);
|
||||
serialize8(masterConfig.gyro_sync_denom);
|
||||
serialize8(masterConfig.pid_process_denom);
|
||||
serialize8(masterConfig.motor_pwm_protocol);
|
||||
serialize16(masterConfig.motor_pwm_rate);
|
||||
break;
|
||||
case MSP_FILTER_CONFIG :
|
||||
headSerialReply(5);
|
||||
serialize8(masterConfig.gyro_soft_lpf_hz);
|
||||
serialize16(currentProfile->pidProfile.dterm_lpf_hz);
|
||||
serialize16(currentProfile->pidProfile.yaw_lpf_hz);
|
||||
break;
|
||||
case MSP_ADVANCED_TUNING:
|
||||
headSerialReply(3 * 3);
|
||||
serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate);
|
||||
serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
|
||||
serialize16(currentProfile->pidProfile.yaw_p_limit);
|
||||
break;
|
||||
case MSP_TEMPORARY_COMMANDS:
|
||||
headSerialReply(1);
|
||||
serialize8(currentControlRateProfile->rcYawRate8);
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
|
|
@ -1689,6 +1712,26 @@ static bool processInCommand(void)
|
|||
|
||||
case MSP_SET_BF_CONFIG:
|
||||
|
||||
case MSP_SET_PID_ADVANCED_CONFIG :
|
||||
masterConfig.gyro_sync_denom = read8();
|
||||
masterConfig.pid_process_denom = read8();
|
||||
masterConfig.motor_pwm_protocol = read8();
|
||||
masterConfig.motor_pwm_rate = read16();
|
||||
break;
|
||||
case MSP_SET_FILTER_CONFIG :
|
||||
masterConfig.gyro_soft_lpf_hz = read8();
|
||||
currentProfile->pidProfile.dterm_lpf_hz = read16();
|
||||
currentProfile->pidProfile.yaw_lpf_hz = read16();
|
||||
break;
|
||||
case MSP_SET_ADVANCED_TUNING:
|
||||
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
|
||||
currentProfile->pidProfile.yawItermIgnoreRate = read16();
|
||||
currentProfile->pidProfile.yaw_p_limit = read16();
|
||||
break;
|
||||
case MSP_SET_TEMPORARY_COMMANDS:
|
||||
currentControlRateProfile->rcYawRate8 = read8();
|
||||
break;
|
||||
|
||||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
read8(); // mixerMode ignored
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -193,6 +193,19 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
// DEPRECATED - Use MSP_BUILD_INFO instead
|
||||
#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
|
||||
|
||||
// Betaflight Additional Commands
|
||||
#define MSP_PID_ADVANCED_CONFIG 90
|
||||
#define MSP_SET_PID_ADVANCED_CONFIG 91
|
||||
|
||||
#define MSP_FILTER_CONFIG 92
|
||||
#define MSP_SET_FILTER_CONFIG 93
|
||||
|
||||
#define MSP_ADVANCED_TUNING 94
|
||||
#define MSP_SET_ADVANCED_TUNING 95
|
||||
|
||||
#define MSP_TEMPORARY_COMMANDS 98 // Temporary Commands before cleanup
|
||||
#define MSP_SET_TEMPORARY_COMMANDS 99 // Temporary Commands before cleanup
|
||||
|
||||
//
|
||||
// Multwii original MSP commands
|
||||
//
|
||||
|
|
|
|||
|
|
@ -123,6 +123,7 @@ extern uint8_t PIDweight[3];
|
|||
uint16_t filteredCycleTime;
|
||||
static bool isRXDataNew;
|
||||
static bool armingCalibrationWasInitialised;
|
||||
float angleRate[3], angleRateSmooth[2];
|
||||
|
||||
extern pidControllerFuncPtr pid_controller;
|
||||
|
||||
|
|
@ -171,12 +172,29 @@ bool isCalibrating()
|
|||
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
|
||||
}
|
||||
|
||||
void filterRc(void)
|
||||
float calculateRate(int axis, int16_t rc) {
|
||||
float angleRate;
|
||||
|
||||
if (isSuperExpoActive()) {
|
||||
float rcFactor = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f)));
|
||||
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
|
||||
|
||||
angleRate = rcFactor * ((27 * rc) / 16.0f);
|
||||
} else {
|
||||
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
|
||||
}
|
||||
|
||||
|
||||
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
|
||||
}
|
||||
|
||||
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;
|
||||
uint16_t rxRefreshRate;
|
||||
int axis;
|
||||
|
||||
// Set RC refresh rate for sampling and channels to filter
|
||||
initRxRefreshRate(&rxRefreshRate);
|
||||
|
|
@ -184,6 +202,8 @@ void filterRc(void)
|
|||
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
|
||||
|
||||
if (isRXDataNew) {
|
||||
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
|
||||
|
||||
for (int channel=0; channel < 4; channel++) {
|
||||
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
|
||||
lastCommand[channel] = rcCommand[channel];
|
||||
|
|
@ -198,8 +218,9 @@ void filterRc(void)
|
|||
// Interpolate steps of rcCommand
|
||||
if (factor > 0) {
|
||||
for (int channel=0; channel < 4; channel++) {
|
||||
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
|
||||
}
|
||||
rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
|
||||
}
|
||||
for (axis = 0; axis < 2; axis++) angleRateSmooth[axis] = calculateRate(axis, rcCommandSmooth[axis]);
|
||||
} else {
|
||||
factor = 0;
|
||||
}
|
||||
|
|
@ -644,7 +665,6 @@ void subTaskPidController(void)
|
|||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(
|
||||
¤tProfile->pidProfile,
|
||||
currentControlRateProfile,
|
||||
masterConfig.max_angle_inclination,
|
||||
&masterConfig.accelerometerTrims,
|
||||
&masterConfig.rxConfig
|
||||
|
|
@ -656,9 +676,7 @@ void subTaskMainSubprocesses(void) {
|
|||
|
||||
const uint32_t startTime = micros();
|
||||
|
||||
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
|
||||
filterRc();
|
||||
}
|
||||
processRcCommand();
|
||||
|
||||
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
|
||||
if (gyro.temperature) {
|
||||
|
|
@ -696,7 +714,8 @@ void subTaskMainSubprocesses(void) {
|
|||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||
#endif
|
||||
) {
|
||||
rcCommand[YAW] = 0;
|
||||
rcCommand[YAW] = rcCommandSmooth[YAW] = 0;
|
||||
angleRate[YAW] = angleRateSmooth[YAW] = 0;
|
||||
}
|
||||
|
||||
if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue