Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Finalisation of Betaflight PID controller // RC smoothing added back

Browse source
This commit is contained in:
borisbstyle 2016-07-27 14:30:51 +02:00
parent 6e99489200
commit c11c50287e
10 changed files with 186 additions and 153 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/config/config.c
View file

@ -239,11 +239,14 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
// Betaflight PID controller parameters // Betaflight PID controller parameters
pidProfile->ptermSetpointWeight = 80;
pidProfile->dtermSetpointWeight = 0;
pidProfile->pidMaxVelocity = 1000;
pidProfile->pidMaxVelocityYaw = 50;
pidProfile->toleranceBand = 15; pidProfile->toleranceBand = 15;
pidProfile->toleranceBandReduction = 35; pidProfile->toleranceBandReduction = 40;
pidProfile->zeroCrossAllowanceCount = 3; pidProfile->zeroCrossAllowanceCount = 2;
pidProfile->accelerationLimitPercent = 20; pidProfile->itermThrottleGain = 15;
pidProfile->itermThrottleGain = 10;
#ifdef GTUNE #ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
@ -524,7 +527,8 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0; masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothInterval = 0; // 0 is predefined masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO;
masterConfig.rxConfig.rcSmoothInterval = 9;
masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.fpvCamAngleDegrees = 0;
#ifdef STM32F4 #ifdef STM32F4
masterConfig.rxConfig.max_aux_channel = 99; masterConfig.rxConfig.max_aux_channel = 99;

3
src/main/debug.h
View file

@ -51,5 +51,8 @@ typedef enum {
DEBUG_AIRMODE, DEBUG_AIRMODE,
DEBUG_PIDLOOP, DEBUG_PIDLOOP,
DEBUG_NOTCH, DEBUG_NOTCH,
DEBUG_RC_SMOOTHING,
DEBUG_VELOCITY,
DEBUG_DTERM_FILTER,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

23
src/main/flight/mixer.c
View file

@ -835,34 +835,11 @@ void mixTable(void *pidProfilePtr)
throttleMax = throttleMax - (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2);
} }
// Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math
float motorDtms;
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static uint32_t previousMotorTime;
uint32_t currentMotorTime = micros();
motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f;
previousMotorTime = currentMotorTime;
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // 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. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
// Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS];
// acceleration limit
float delta = motor[i] - lastFilteredMotor[i];
const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f);
float maxDelta = maxDeltaPerMs * motorDtms;
if (delta > maxDelta) { // accelerating too hard
motor[i] = lastFilteredMotor[i] + maxDelta;
}
lastFilteredMotor[i] = motor[i];
}
if (isFailsafeActive) { if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) { } else if (feature(FEATURE_3D)) {

152
src/main/flight/pid.c
View file

@ -49,7 +49,8 @@
extern uint8_t motorCount; extern uint8_t motorCount;
uint32_t targetPidLooptime; uint32_t targetPidLooptime;
extern float angleRate[3], angleRateSmooth[3]; extern float setpointRate[3];
extern float rcInput[3];
static bool pidStabilisationEnabled; static bool pidStabilisationEnabled;
@ -106,25 +107,23 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static pt1Filter_t deltaFilter[3]; static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter; static pt1Filter_t yawFilter;
#ifndef SKIP_PID_FLOAT // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab)
// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{ {
float RateError = 0, gyroRate = 0, RateErrorSmooth = 0; float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRateError[2]; static float lastRateError[2];
float delta; float delta;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
static int16_t axisPIDState[3];
static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f };
const float velocityFactor = getdT() * 1000.0f;
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
// Scaling factors for Pids for better tunable range in configurator
static const float PTermScale = 0.032029f;
static const float ITermScale = 0.244381f;
static const float DTermScale = 0.000529f;
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions // Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -142,7 +141,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// Yet Highly experimental and under test and development // Yet Highly experimental and under test and development
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
static float kiThrottleGain = 1.0f; static float kiThrottleGain = 1.0f;
if (pidProfile->itermThrottleGain) { if (pidProfile->itermThrottleGain) {
const uint16_t maxLoopCount = 20000 / targetPidLooptime; const uint16_t maxLoopCount = 20000 / targetPidLooptime;
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
@ -161,126 +159,136 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// Prepare all parameters for PID controller
float Kp = PTERM_SCALE * pidProfile->P8[axis];
float Ki = ITERM_SCALE * pidProfile->I8[axis];
float Kd = DTERM_SCALE * pidProfile->D8[axis];
float b = pidProfile->ptermSetpointWeight / 100.0f;
float c = pidProfile->dtermSetpointWeight / 100.0f;
float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor;
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination // calculate error angle and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else #else
const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed // ANGLE mode - control is angle based, so control loop is needed
angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else { } else {
// HORIZON mode - direct sticks control is applied to rate PID // HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
} }
} }
gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates // ----- calculate error / angle rates ----------
// multiplication of rcCommand corresponds to changing the sticks scaling here errorRate = setpointRate[axis] - PVRate; // r - y
RateError = angleRate[axis] - gyroRate; rP = b * setpointRate[axis] - PVRate; // br - y
rD = c * setpointRate[axis] - PVRate; // cr - y
// Slowly restore original setpoint with more stick input
float diffRate = errorRate - rP;
rP += diffRate * rcInput[axis];
float dynReduction = tpaFactor;
// Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
float dynReduction = tpaFactor;
if (pidProfile->toleranceBand) { if (pidProfile->toleranceBand) {
const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f;
static uint8_t zeroCrossCount[3]; static uint8_t zeroCrossCount[3];
static uint8_t currentErrorPolarity[3]; static uint8_t currentErrorPolarity[3];
if (ABS(RateError) < pidProfile->toleranceBand) { if (ABS(errorRate) < pidProfile->toleranceBand) {
if (zeroCrossCount[axis]) { if (zeroCrossCount[axis]) {
if (currentErrorPolarity[axis] == POSITIVE_ERROR) { if (currentErrorPolarity[axis] == POSITIVE_ERROR) {
if (RateError < 0 ) { if (errorRate < 0 ) {
zeroCrossCount[axis]--; zeroCrossCount[axis]--;
currentErrorPolarity[axis] = NEGATIVE_ERROR; currentErrorPolarity[axis] = NEGATIVE_ERROR;
} }
} else { } else {
if (RateError > 0 ) { if (errorRate > 0 ) {
zeroCrossCount[axis]--; zeroCrossCount[axis]--;
currentErrorPolarity[axis] = POSITIVE_ERROR; currentErrorPolarity[axis] = POSITIVE_ERROR;
} }
} }
} else { } else {
dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f); dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f);
} }
} else { } else {
zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount;
currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR;
} }
} }
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
RateErrorSmooth = RateError;
else
RateErrorSmooth = angleRateSmooth[axis] - gyroRate;
}
// -----calculate P component // -----calculate P component
PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction; PTerm = Kp * rP * dynReduction;
// 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) {
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
// -----calculate I component. // -----calculate I component.
// Prevent strong Iterm accumulation during stick inputs // Reduce strong Iterm accumulation during higher stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f); // Handle All windup Scenarios
// limit maximum integrator value to prevent WindUp
float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis];
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -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 // I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis]; ITerm = errorGyroIf[axis];
//-----calculate D-term //-----calculate D-term (Yaw D not yet supported)
if (axis == YAW) { if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = lrintf(PTerm + ITerm); axisPID[axis] = lrintf(PTerm + ITerm);
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0.0f; // needed for blackbox DTerm = 0.0f; // needed for blackbox
} else { } else {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { delta = rD - lastRateError[axis];
delta = RateErrorSmooth - lastRateError[axis]; lastRateError[axis] = rD;
lastRateError[axis] = RateErrorSmooth;
} else {
delta = -(gyroRate - lastRateError[axis]);
lastRateError[axis] = gyroRate;
}
// Divide delta by targetLooptime to get differential (ie dr/dt) // Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT()); delta *= (1.0f / getdT());
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction;
// Filter delta // Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f); DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f);
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
} }
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPID[axis] = 0; if (!pidStabilisationEnabled) axisPID[axis] = 0;
// Velocity limit only active below 1000
if (velocityMax < 1000) {
int16_t currentVelocity = axisPID[axis] - axisPIDState[axis];
if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity;
if (ABS(currentVelocity) > velocityMax) {
axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax;
velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax;
}
else {
velocityWindupFactor[axis] = 1.0f;
}
axisPIDState[axis] = axisPID[axis];
}
#ifdef GTUNE #ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis); calculate_Gtune(axis);
@ -294,7 +302,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
#endif #endif
} }
} }
#endif
// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future // Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
@ -303,7 +310,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
int axis; int axis;
int32_t PTerm, ITerm, DTerm, delta; int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRateError[3]; static int32_t lastRateError[3];
int32_t AngleRateTmp = 0, AngleRateTmpSmooth = 0, RateError = 0, gyroRate = 0, RateErrorSmooth = 0; int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -323,16 +330,15 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
AngleRateTmp = (int32_t)angleRate[axis]; AngleRateTmp = (int32_t)setpointRate[axis];
if (axis != YAW) AngleRateTmpSmooth = (int32_t)angleRateSmooth[axis];
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to max configured inclination // calculate error angle and limit the angle to max configured inclination
#ifdef GPS #ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
@ -341,7 +347,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
} else { } else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input // horizonLevelStrength is scaled to the stick input
AngleRateTmp = AngleRateTmpSmooth + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4);
} }
} }
@ -353,14 +359,6 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
RateError = AngleRateTmp - gyroRate; RateError = AngleRateTmp - gyroRate;
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
RateErrorSmooth = RateError;
else
RateErrorSmooth = AngleRateTmpSmooth - gyroRate;
}
// -----calculate P component // -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@ -403,8 +401,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
DTerm = 0; // needed for blackbox DTerm = 0; // needed for blackbox
} else { } else {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateErrorSmooth - lastRateError[axis]; delta = RateError - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth; lastRateError[axis] = RateError;
} else { } else {
delta = -(gyroRate - lastRateError[axis]); delta = -(gyroRate - lastRateError[axis]);
lastRateError[axis] = gyroRate; lastRateError[axis] = gyroRate;
@ -413,6 +411,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// Divide delta by targetLooptime to get differential (ie dr/dt) // Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// Filter delta // Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());

11
src/main/flight/pid.h
View file

@ -26,6 +26,12 @@
#define DYNAMIC_PTERM_STICK_THRESHOLD 400 #define DYNAMIC_PTERM_STICK_THRESHOLD 400
// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
#define PTERM_SCALE 0.032029f
#define ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f
typedef enum { typedef enum {
PIDROLL, PIDROLL,
PIDPITCH, PIDPITCH,
@ -88,8 +94,11 @@ typedef struct pidProfile_s {
uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances
uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage
uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in
uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking)
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
uint16_t pidMaxVelocity; // velocity limiter for pid controller (per ms)
uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller (per ms) yaw
#ifdef GTUNE #ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

7
src/main/io/rc_controls.h
View file

@ -86,6 +86,13 @@ typedef enum {
CENTERED CENTERED
} rollPitchStatus_e; } rollPitchStatus_e;
typedef enum {
RC_SMOOTHING_OFF = 0,
RC_SMOOTHING_DEFAULT,
RC_SMOOTHING_AUTO,
RC_SMOOTHING_MANUAL
} rcSmoothing_t;
#define ROL_LO (1 << (2 * ROLL)) #define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL))

21
src/main/io/serial_cli.c
View file

@ -468,6 +468,9 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"AIRMODE", "AIRMODE",
"PIDLOOP", "PIDLOOP",
"NOTCH", "NOTCH",
"RC_SMOOTHING",
"VELOCITY",
"DFILTER",
}; };
#ifdef OSD #ifdef OSD
@ -486,10 +489,14 @@ static const char * const lookupTablePwmProtocol[] = {
"OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED"
}; };
static const char * const lookupDeltaMethod[] = { static const char * const lookupTableDeltaMethod[] = {
"ERROR", "MEASUREMENT" "ERROR", "MEASUREMENT"
}; };
static const char * const lookupTableRcSmoothing[] = {
"OFF", "DEFAULT", "AUTO", "MANUAL"
};
typedef struct lookupTableEntry_s { typedef struct lookupTableEntry_s {
const char * const *values; const char * const *values;
const uint8_t valueCount; const uint8_t valueCount;
@ -526,6 +533,7 @@ typedef enum {
TABLE_SUPEREXPO_YAW, TABLE_SUPEREXPO_YAW,
TABLE_MOTOR_PWM_PROTOCOL, TABLE_MOTOR_PWM_PROTOCOL,
TABLE_DELTA_METHOD, TABLE_DELTA_METHOD,
TABLE_RC_SMOOTHING,
#ifdef OSD #ifdef OSD
TABLE_OSD, TABLE_OSD,
#endif #endif
@ -561,7 +569,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
{ lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
{ lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
#ifdef OSD #ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif #endif
@ -621,7 +630,8 @@ const clivalue_t valueTable[] = {
{ "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
{ "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 0, 255 } }, { "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_RC_SMOOTHING } },
{ "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 1, 50 } },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, { "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 } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
@ -807,11 +817,14 @@ const clivalue_t valueTable[] = {
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } },
{ "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } },
{ "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },
{ "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } },
{ "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
{ "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } },
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } },
{ "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } },
{ "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "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_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },

4
src/main/io/serial_msp.c
View file

@ -1248,7 +1248,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentControlRateProfile->rcYawRate8); serialize8(currentControlRateProfile->rcYawRate8);
serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize16(masterConfig.rxConfig.airModeActivateThreshold);
serialize8(masterConfig.rxConfig.rcSmoothInterval); serialize8(masterConfig.rxConfig.rcSmoothInterval);
serialize16(currentProfile->pidProfile.accelerationLimitPercent); serialize16(currentProfile->pidProfile.pidMaxVelocity);
break; break;
case MSP_SENSOR_CONFIG: case MSP_SENSOR_CONFIG:
headSerialReply(3); headSerialReply(3);
@ -1829,7 +1829,7 @@ static bool processInCommand(void)
currentControlRateProfile->rcYawRate8 = read8(); currentControlRateProfile->rcYawRate8 = read8();
masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.airModeActivateThreshold = read16();
masterConfig.rxConfig.rcSmoothInterval = read8(); masterConfig.rxConfig.rcSmoothInterval = read8();
currentProfile->pidProfile.accelerationLimitPercent = read16(); read16();
break; break;
case MSP_SET_SENSOR_CONFIG: case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8(); masterConfig.acc_hardware = read8();

103
src/main/mw.c
View file

@ -123,7 +123,8 @@ extern uint8_t PIDweight[3];
uint16_t filteredCycleTime; uint16_t filteredCycleTime;
static bool isRXDataNew; static bool isRXDataNew;
static bool armingCalibrationWasInitialised; static bool armingCalibrationWasInitialised;
float angleRate[3], angleRateSmooth[3]; float setpointRate[3];
float rcInput[3];
extern pidControllerFuncPtr pid_controller; extern pidControllerFuncPtr pid_controller;
@ -172,12 +173,12 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
} }
float calculateRate(int axis, int16_t rc) { float calculateSetpointRate(int axis, int16_t rc) {
float angleRate; float angleRate;
if (isSuperExpoActive()) { if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); rcInput[axis] = (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)); float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rc) / 16.0f); angleRate = rcFactor * ((27 * rc) / 16.0f);
} else { } else {
@ -202,10 +203,10 @@ void scaleRcCommandToFpvCamAngle(void) {
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
} }
int16_t roll = angleRate[ROLL]; int16_t roll = setpointRate[ROLL];
int16_t yaw = angleRate[YAW]; int16_t yaw = setpointRate[YAW];
angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); setpointRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); setpointRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
} }
void processRcCommand(void) void processRcCommand(void)
@ -214,44 +215,62 @@ void processRcCommand(void)
static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor; static int16_t factor, rcInterpolationFactor;
uint16_t rxRefreshRate; uint16_t rxRefreshRate;
int axis; bool readyToCalculateRate = false;
// Set RC refresh rate for sampling and channels to filter if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
if (masterConfig.rxConfig.rcSmoothInterval) { if (isRXDataNew) {
rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval; // Set RC refresh rate for sampling and channels to filter
switch (masterConfig.rxConfig.rcSmoothing) {
case(RC_SMOOTHING_AUTO):
rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
break;
case(RC_SMOOTHING_MANUAL):
rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval;
break;
case(RC_SMOOTHING_OFF):
case(RC_SMOOTHING_DEFAULT):
default:
initRxRefreshRate(&rxRefreshRate);
}
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (debugMode == DEBUG_RC_SMOOTHING) {
for (int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
debug[3] = rxRefreshRate;
}
isRXDataNew = false;
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
} else {
factor = 0;
}
readyToCalculateRate = true;
} else { } else {
initRxRefreshRate(&rxRefreshRate); factor = 0; // reset factor in case of level modes flip flopping
} }
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; if (readyToCalculateRate || isRXDataNew) {
isRXDataNew = false;
if (isRXDataNew) { for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw) // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle(); scaleRcCommandToFpvCamAngle();
}
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
isRXDataNew = false;
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
for (axis = 0; axis < 2; axis++) angleRateSmooth[axis] = calculateRate(axis, rcCommandSmooth[axis]);
} else {
factor = 0;
} }
} }
@ -688,8 +707,6 @@ void subTaskMainSubprocesses(void) {
const uint32_t startTime = micros(); const uint32_t startTime = micros();
processRcCommand();
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) { if (gyro.temperature) {
gyro.temperature(&telemTemperature1); gyro.temperature(&telemTemperature1);
@ -728,14 +745,16 @@ void subTaskMainSubprocesses(void) {
&& masterConfig.mixerMode != MIXER_FLYING_WING && masterConfig.mixerMode != MIXER_FLYING_WING
#endif #endif
) { ) {
rcCommand[YAW] = rcCommandSmooth[YAW] = 0; rcCommand[YAW] = 0;
angleRate[YAW] = angleRateSmooth[YAW] = 0; setpointRate[YAW] = 0;
} }
if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
} }
processRcCommand();
#ifdef GPS #ifdef GPS
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {

1
src/main/rx/rx.h
View file

@ -121,6 +121,7 @@ typedef struct rxConfig_s {
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end uint16_t maxcheck; // maximum rc end
uint8_t rcSmoothing;
uint8_t rcSmoothInterval; uint8_t rcSmoothInterval;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel; uint8_t max_aux_channel;