diff --git a/docs/Settings.md b/docs/Settings.md index 256dd05a005..193cc38485a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -302,6 +302,16 @@ ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't hav --- +### airspeed_tpa_pow + +Use airspeed instead of throttle position for TPA if airspeed is available on fixedwing. pid_multiplier(tpa_factor) = (referenceAirspeed/airspeed)**(airspeed_tpa_pow/100). Set to 0 will disable this feature and use throttle based tpa; + +| Default | Min | Max | +| --- | --- | --- | +| 120 | 0 | 200 | + +--- + ### align_board_pitch Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc @@ -5568,7 +5578,7 @@ Pitot tube lowpass filter cutoff frequency. Lower cutoff frequencies result in s | Default | Min | Max | | --- | --- | --- | -| 350 | 0 | 10000 | +| 1000 | 0 | 10000 | --- diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c8b144e136c..1c2d632d9ce 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -46,7 +46,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_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, + .airspeed_tpa_pow = SETTING_AIRSPEED_TPA_POW_DEFAULT, }, .stabilized = { diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index e4038537603..8c4e57b7f1f 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -32,6 +32,7 @@ typedef struct controlRateConfig_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 airspeed_tpa_pow; // Use airspeed instead of throttle position for TPA calculation,0 to disable } throttle; struct { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5fe29261630..d5a80e745d3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -656,7 +656,7 @@ groups: description: "Pitot tube lowpass filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay" min: 0 max: 10000 - default_value: 350 + default_value: 1000 - name: pitot_scale description: "Pitot tube scale factor" min: 0 @@ -1337,6 +1337,13 @@ groups: field: throttle.rcExpo8 min: 0 max: 100 + - name: airspeed_tpa_pow + description: "Use airspeed instead of throttle position for TPA if airspeed is available on fixedwing. pid_multiplier(tpa_factor) = (referenceAirspeed/airspeed)**(airspeed_tpa_pow/100). Set to 0 will disable this feature and use throttle based tpa;" + type: uint16_t + field: throttle.airspeed_tpa_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." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae8eb57fb30..14a78ea7842 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 @@ -441,6 +441,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), currentControlRateProfile->throttle.airspeed_tpa_pow/100.0f); + tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); + return tpaFactor; +} + static float calculateFixedWingTPAFactor(uint16_t throttle) { float tpaFactor; @@ -469,15 +477,15 @@ 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 (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) { + if (currentControlRateProfile->throttle.dynPID == 0 || throttle < currentControlRateProfile->throttle.pa_breakpoint) { tpaFactor = 1.0f; - } else if (rcCommand[THROTTLE] < getMaxThrottle()) { - tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; + } else if (throttle < getMaxThrottle()) { + tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (throttle - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; } else { tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; } @@ -485,6 +493,19 @@ static float calculateMultirotorTPAFactor(void) return tpaFactor; } +static float calculateTPAThtrottle(void) +{ + uint16_t tpaThrottle = 0; + + if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering + tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); + } + else { + tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering + } + return tpaThrottle; +} + void schedulePidGainsUpdate(void) { pidGainsUpdateRequired = true; @@ -492,22 +513,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 && (currentControlRateProfile->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) { @@ -522,14 +528,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(currentControlRateProfile->throttle.airspeed_tpa_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 baf7bed0a61..cb5b9f76f9a 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -305,4 +305,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