diff --git a/docs/Settings.md b/docs/Settings.md index b44f1171df0..b886f6702ac 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -432,6 +432,16 @@ Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allo --- +### apa_pow + +Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation; + +| Default | Min | Max | +| --- | --- | --- | +| 120 | 0 | 200 | + +--- + ### applied_defaults Internal (configurator) hint. Should not be changed manually @@ -6442,13 +6452,23 @@ Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should b --- +### tpa_pitch_compensation + +Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down. + +| Default | Min | Max | +| --- | --- | --- | +| 8 | 0 | 20 | + +--- + ### tpa_rate -Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. +Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 100 | +| 0 | 0 | 200 | --- diff --git a/src/main/fc/control_profile.c b/src/main/fc/control_profile.c index ec9f9108325..316643b343f 100644 --- a/src/main/fc/control_profile.c +++ b/src/main/fc/control_profile.c @@ -45,7 +45,9 @@ void pgResetFn_controlProfiles(controlConfig_t *instance) .dynPID = SETTING_TPA_RATE_DEFAULT, .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, - .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT + .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT, + .apa_pow = SETTING_APA_POW_DEFAULT, + .tpa_pitch_compensation = SETTING_TPA_PITCH_COMPENSATION_DEFAULT }, .stabilized = { diff --git a/src/main/fc/control_profile_config_struct.h b/src/main/fc/control_profile_config_struct.h index 1ad41773f83..9300858fe36 100644 --- a/src/main/fc/control_profile_config_struct.h +++ b/src/main/fc/control_profile_config_struct.h @@ -32,6 +32,8 @@ typedef struct controlConfig_s { bool dynPID_on_YAW; uint16_t pa_breakpoint; // Breakpoint where TPA is activated uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter + uint16_t apa_pow; // Use airspeed instead of throttle position for TPA calculation,0 to disable + uint8_t tpa_pitch_compensation; // Pitch angle based throttle compensation for fixed wing } throttle; struct { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c871e9caeac..01ca6149bfb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1379,18 +1379,31 @@ groups: field: throttle.rcExpo8 min: 0 max: 100 + - name: apa_pow + description: "Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation;" + type: uint16_t + field: throttle.apa_pow + default_value: 120 + min: 0 + max: 200 - name: tpa_rate - description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." + description: "Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details." default_value: 0 field: throttle.dynPID min: 0 - max: 100 + max: 200 - name: tpa_breakpoint description: "See tpa_rate." default_value: 1500 field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX + - name: tpa_pitch_compensation + description: "Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down." + default_value: 8 + field: throttle.tpa_pitch_compensation + min: 0 + max: 20 - name: tpa_on_yaw description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors." type: bool diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ecadcfbd7e4..7a112ff8f4f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -126,7 +126,7 @@ static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied -STATIC_FASTRAM bool pidGainsUpdateRequired; +STATIC_FASTRAM bool pidGainsUpdateRequired= true; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; #ifdef USE_BLACKBOX @@ -442,6 +442,14 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate) return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS); } +static float calculateFixedWingAirspeedTPAFactor(void){ + const float airspeed = getAirspeedEstimate(); // in cm/s + const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s + float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); + tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); + return tpaFactor; +} + static float calculateFixedWingTPAFactor(uint16_t throttle) { float tpaFactor; @@ -452,9 +460,6 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle tpaFactor = 0.5f + ((float)(currentControlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); - - // Limit to [0.5; 2] range - tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f); } else { tpaFactor = 2.0f; @@ -462,6 +467,8 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // Attenuate TPA curve according to configured amount tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlProfile->throttle.dynPID / 100.0f); + // Limit to [0.5; 2] range + tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f); } else { tpaFactor = 1.0f; @@ -470,22 +477,40 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) return tpaFactor; } -static float calculateMultirotorTPAFactor(void) +static float calculateMultirotorTPAFactor(uint16_t throttle) { float tpaFactor; // TPA should be updated only when TPA is actually set - if (currentControlProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlProfile->throttle.pa_breakpoint) { + if (currentControlProfile->throttle.dynPID == 0 || throttle < currentControlProfile->throttle.pa_breakpoint) { tpaFactor = 1.0f; - } else if (rcCommand[THROTTLE] < getMaxThrottle()) { - tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f; + } else if (throttle < getMaxThrottle()) { + tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (throttle - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f; } else { - tpaFactor = (100 - currentControlProfile->throttle.dynPID) / 100.0f; + tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f; } return tpaFactor; } +static float calculateTPAThtrottle(void) +{ + uint16_t tpaThrottle = 0; + static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } }; + + if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering + fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; + float groundCos = vectorDotProduct(&vForward, &vDown); + int16_t throttleAdjustment = currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //when 1deg pitch up, increase throttle by pitch(deg)_to_throttle. cos(89 deg)*90/(pi/2)=0.99995,cos(80 deg)*90/(pi/2)=9.9493, + uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); + tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, constrain(throttleAdjusted, 1000, 2000)); + } + else { + tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering + } + return tpaThrottle; +} + void schedulePidGainsUpdate(void) { pidGainsUpdateRequired = true; @@ -493,22 +518,7 @@ void schedulePidGainsUpdate(void) void updatePIDCoefficients(void) { - STATIC_FASTRAM uint16_t prevThrottle = 0; - - // Check if throttle changed. Different logic for fixed wing vs multirotor - if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { - uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); - if (filteredThrottle != prevThrottle) { - prevThrottle = filteredThrottle; - pidGainsUpdateRequired = true; - } - } - else { - if (rcCommand[THROTTLE] != prevThrottle) { - prevThrottle = rcCommand[THROTTLE]; - pidGainsUpdateRequired = true; - } - } + STATIC_FASTRAM float tpaFactorprev=-1.0f; #ifdef USE_ANTIGRAVITY if (usedPidControllerType == PID_TYPE_PID) { @@ -523,14 +533,29 @@ void updatePIDCoefficients(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } + + float tpaFactor=1.0f; + if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation + if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){ + tpaFactor = calculateFixedWingAirspeedTPAFactor(); + }else{ + tpaFactor = calculateFixedWingTPAFactor(calculateTPAThtrottle()); + } + } else { + tpaFactor = calculateMultirotorTPAFactor(calculateTPAThtrottle()); + } + if (tpaFactor != tpaFactorprev) { + pidGainsUpdateRequired = true; + } + tpaFactorprev = tpaFactor; + // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; } - const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); - + // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 65678ae5ecb..edcc5b9219d 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -312,4 +312,13 @@ bool pitotIsHealthy(void) return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; } +bool pitotValidForAirspeed(void) +{ + bool ret = false; + ret = pitotIsHealthy() && pitotIsCalibrationComplete(); + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + ret = ret && STATE(GPS_FIX); + } + return ret; +} #endif /* PITOT */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index aed924f8e4c..68f5a9c1a02 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -69,5 +69,6 @@ void pitotStartCalibration(void); void pitotUpdate(void); float getAirspeedEstimate(void); bool pitotIsHealthy(void); +bool pitotValidForAirspeed(void); #endif