diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 45d40a44db..4c8d4f539b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1409,6 +1409,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt, motorOutputHighInt); + BLACKBOX_PRINT_HEADER_LINE("motor_kv", "%d", motorConfig()->kv); #if defined(USE_ACC) BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); #endif @@ -1737,9 +1738,15 @@ static bool blackboxWriteSysinfo(void) #endif #ifdef USE_WING - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR0, "%d", currentPidProfile->tpa_gravity_thr0); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR100, "%d", currentPidProfile->tpa_gravity_thr100); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_TYPE, "%d", currentPidProfile->tpa_speed_type); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_DELAY, "%d", currentPidProfile->tpa_speed_basic_delay); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_GRAVITY, "%d", currentPidProfile->tpa_speed_basic_gravity); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH, "%d", currentPidProfile->tpa_speed_adv_prop_pitch); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_MASS, "%d", currentPidProfile->tpa_speed_adv_mass); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_DRAG_K, "%d", currentPidProfile->tpa_speed_adv_drag_k); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_THRUST, "%d", currentPidProfile->tpa_speed_adv_thrust); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, "%d", currentPidProfile->tpa_speed_max_voltage); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_PITCH_OFFSET, "%d", currentPidProfile->tpa_speed_pitch_offset); #endif default: diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 5d57255852..eff21b6f1b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -539,6 +539,12 @@ const char* const lookupTableTpaCurveType[] = { }; #endif +#ifdef USE_WING +const char* const lookupTableTpaSpeedType[] = { + "BASIC", "ADVANCED", +}; +#endif + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -665,6 +671,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_ADVANCED_TPA LOOKUP_TABLE_ENTRY(lookupTableTpaCurveType), #endif +#ifdef USE_WING + LOOKUP_TABLE_ENTRY(lookupTableTpaSpeedType), +#endif }; #undef LOOKUP_TABLE_ENTRY @@ -1278,9 +1287,15 @@ const clivalue_t valueTable[] = { { PARAM_NAME_TPA_LOW_ALWAYS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_always) }, #ifdef USE_WING - { PARAM_NAME_TPA_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_delay_ms) }, - { PARAM_NAME_TPA_GRAVITY_THR0, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr0) }, - { PARAM_NAME_TPA_GRAVITY_THR100, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr100) }, + { PARAM_NAME_TPA_SPEED_TYPE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_SPEED_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_type) }, + { PARAM_NAME_TPA_SPEED_BASIC_DELAY, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_basic_delay) }, + { PARAM_NAME_TPA_SPEED_BASIC_GRAVITY, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_basic_gravity) }, + { PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_prop_pitch) }, + { PARAM_NAME_TPA_SPEED_ADV_MASS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_mass) }, + { PARAM_NAME_TPA_SPEED_ADV_DRAG_K, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_drag_k) }, + { PARAM_NAME_TPA_SPEED_ADV_THRUST, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_thrust) }, + { PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_max_voltage) }, + { PARAM_NAME_TPA_SPEED_PITCH_OFFSET, VAR_INT16 | PROFILE_VALUE, .config.minmaxUnsigned = { INT16_MIN, INT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_pitch_offset) }, #endif // USE_WING #ifdef USE_ADVANCED_TPA diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 17f5e94e8e..f0fea366c9 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -145,6 +145,9 @@ typedef enum { #endif #ifdef USE_ADVANCED_TPA TABLE_TPA_CURVE_TYPE, +#endif +#ifdef USE_WING + TABLE_TPA_SPEED_TYPE, #endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 39118b40c3..47fa7cf980 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -58,14 +58,20 @@ #define PARAM_NAME_TPA_LOW_BREAKPOINT "tpa_low_breakpoint" #define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always" #define PARAM_NAME_TPA_MODE "tpa_mode" -#define PARAM_NAME_TPA_DELAY_MS "tpa_delay_ms" -#define PARAM_NAME_TPA_GRAVITY_THR0 "tpa_gravity_thr0" -#define PARAM_NAME_TPA_GRAVITY_THR100 "tpa_gravity_thr100" #define PARAM_NAME_TPA_CURVE_TYPE "tpa_curve_type" #define PARAM_NAME_TPA_CURVE_STALL_THROTTLE "tpa_curve_stall_throttle" #define PARAM_NAME_TPA_CURVE_PID_THR0 "tpa_curve_pid_thr0" #define PARAM_NAME_TPA_CURVE_PID_THR100 "tpa_curve_pid_thr100" #define PARAM_NAME_TPA_CURVE_EXPO "tpa_curve_expo" +#define PARAM_NAME_TPA_SPEED_TYPE "tpa_speed_type" +#define PARAM_NAME_TPA_SPEED_BASIC_DELAY "tpa_speed_basic_delay" +#define PARAM_NAME_TPA_SPEED_BASIC_GRAVITY "tpa_speed_basic_gravity" +#define PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH "tpa_speed_adv_prop_pitch" +#define PARAM_NAME_TPA_SPEED_ADV_MASS "tpa_speed_adv_mass" +#define PARAM_NAME_TPA_SPEED_ADV_DRAG_K "tpa_speed_adv_drag_k" +#define PARAM_NAME_TPA_SPEED_ADV_THRUST "tpa_speed_adv_thrust" +#define PARAM_NAME_TPA_SPEED_MAX_VOLTAGE "tpa_speed_max_voltage" +#define PARAM_NAME_TPA_SPEED_PITCH_OFFSET "tpa_speed_pitch_offset" #define PARAM_NAME_MIXER_TYPE "mixer_type" #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 7d8fff3f54..7f9a1f0b21 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -438,6 +438,15 @@ static void applyRpmLimiter(mixerRuntime_t *mixer) } #endif // USE_RPM_LIMIT +#ifdef USE_WING +static float motorOutputRms = 0.0f; + +float getMotorOutputRms(void) +{ + return motorOutputRms; +} +#endif // USE_WING + static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted @@ -473,6 +482,20 @@ static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS], motorMi motor[i] = motor_disarmed[i]; } } + +#ifdef USE_WING + float motorSumSquares = 0.0f; + if (motorIsEnabled()) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { + if (motorIsMotorEnabled(i)) { + const float motorOutput = scaleRangef(motorConvertToExternal(motor[i]), PWM_RANGE_MIN, PWM_RANGE_MAX, 0.0f, 1.0f); + motorSumSquares += motorOutput * motorOutput; + } + } + } + motorOutputRms = sqrtf(motorSumSquares / mixerRuntime.motorCount); +#endif // USE_WING + DEBUG_SET(DEBUG_EZLANDING, 1, throttle * 10000U); // DEBUG_EZLANDING 0 is the ezLanding factor 2 is the throttle limit } @@ -497,6 +520,10 @@ static void applyMotorStop(void) for (int i = 0; i < mixerRuntime.motorCount; i++) { motor[i] = mixerRuntime.disarmMotorOutput; } + +#ifdef USE_WING + motorOutputRms = 0.0f; +#endif // USE_WING } #ifdef USE_DYN_LPF diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index adefa34a5f..7371f9bdee 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -148,4 +148,9 @@ bool isFixedWing(void); float getMotorOutputLow(void); float getMotorOutputHigh(void); + bool crashFlipSuccessful(void); + +#ifdef USE_WING +float getMotorOutputRms(void); +#endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b484a500eb..f2a5f83288 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -231,9 +231,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .ez_landing_threshold = 25, .ez_landing_limit = 15, .ez_landing_speed = 50, - .tpa_delay_ms = 0, - .tpa_gravity_thr0 = 0, - .tpa_gravity_thr100 = 0, .spa_center = { 0, 0, 0 }, .spa_width = { 0, 0, 0 }, .spa_mode = { 0, 0, 0 }, @@ -245,6 +242,15 @@ void resetPidProfile(pidProfile_t *pidProfile) .tpa_curve_pid_thr0 = 200, .tpa_curve_pid_thr100 = 70, .tpa_curve_expo = 20, + .tpa_speed_type = TPA_SPEED_BASIC, + .tpa_speed_basic_delay = 1000, + .tpa_speed_basic_gravity = 50, + .tpa_speed_adv_prop_pitch = 370, + .tpa_speed_adv_mass = 1000, + .tpa_speed_adv_drag_k = 1000, + .tpa_speed_adv_thrust = 2000, + .tpa_speed_max_voltage = 2520, + .tpa_speed_pitch_offset = 0, ); } @@ -288,21 +294,55 @@ void pidResetIterm(void) } #ifdef USE_WING -static float getWingTpaArgument(float throttle) +static float calcWingThrottle(void) { - const float pitchFactorAdjustment = scaleRangef(throttle, 0.0f, 1.0f, pidRuntime.tpaGravityThr0, pidRuntime.tpaGravityThr100); - const float pitchAngleFactor = getSinPitchAngle() * pitchFactorAdjustment; - DEBUG_SET(DEBUG_TPA, 1, lrintf(pitchAngleFactor * 1000.0f)); + float batteryThrottleFactor = 1.0f; + if (pidRuntime.tpaSpeed.maxVoltage > 0.0f) { + batteryThrottleFactor = getBatteryVoltageLatest() / 100.0f / pidRuntime.tpaSpeed.maxVoltage; + batteryThrottleFactor = constrainf(batteryThrottleFactor, 0.0f, 1.0f); + } - float tpaArgument = throttle + pitchAngleFactor; - const float maxTpaArgument = MAX(1.0 + pidRuntime.tpaGravityThr100, pidRuntime.tpaGravityThr0); - tpaArgument = tpaArgument / maxTpaArgument; - tpaArgument = pt2FilterApply(&pidRuntime.tpaLpf, tpaArgument); - DEBUG_SET(DEBUG_TPA, 2, lrintf(tpaArgument * 1000.0f)); + return getMotorOutputRms() * batteryThrottleFactor; +} + +static float calcWingAcceleration(float throttle, float pitchAngleRadians) +{ + const tpaSpeedParams_t *tpa = &pidRuntime.tpaSpeed; + + const float thrust = (throttle * throttle - throttle * tpa->speed * tpa->inversePropMaxSpeed) * tpa->twr * G_ACCELERATION; + const float drag = tpa->speed * tpa->speed * tpa->dragMassRatio; + const float gravity = G_ACCELERATION * sin_approx(pitchAngleRadians); + + return thrust - drag + gravity; +} + +static float calcWingTpaArgument(void) +{ + const float t = calcWingThrottle(); + const float pitchRadians = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + const float rollRadians = DECIDEGREES_TO_RADIANS(attitude.values.roll); + + DEBUG_SET(DEBUG_TPA, 1, lrintf(attitude.values.roll)); // decidegrees + DEBUG_SET(DEBUG_TPA, 2, lrintf(attitude.values.pitch)); // decidegrees + DEBUG_SET(DEBUG_TPA, 3, lrintf(t * 1000.0f)); // calculated throttle in the range of 0 - 1000 + + // pitchRadians is always -90 to 90 degrees. The bigger the ABS(pitch) the less portion of pitchOffset is needed. + // If ABS(roll) > 90 degrees - flying inverted, then negative portion of pitchOffset is needed. + // If ABS(roll) ~ 90 degrees - flying sideways, no pitchOffset is applied. + const float correctedPitchAnge = pitchRadians + cos_approx(pitchRadians) * cos_approx(rollRadians) * pidRuntime.tpaSpeed.pitchOffset; + + const float a = calcWingAcceleration(t, correctedPitchAnge); + + pidRuntime.tpaSpeed.speed += a * pidRuntime.dT; + pidRuntime.tpaSpeed.speed = MAX(0.0f, pidRuntime.tpaSpeed.speed); + const float tpaArgument = constrainf(pidRuntime.tpaSpeed.speed / pidRuntime.tpaSpeed.maxSpeed, 0.0f, 1.0f); + + DEBUG_SET(DEBUG_TPA, 4, lrintf(pidRuntime.tpaSpeed.speed * 10.0f)); + DEBUG_SET(DEBUG_TPA, 5, lrintf(tpaArgument * 1000.0f)); return tpaArgument; } -#endif // #ifndef USE_WING +#endif // USE_WING float getTpaFactorClassic(float tpaArgument) { @@ -328,7 +368,7 @@ void pidUpdateTpaFactor(float throttle) float tpaFactor; #ifdef USE_WING - const float tpaArgument = isFixedWing() ? getWingTpaArgument(throttle) : throttle; + const float tpaArgument = isFixedWing() ? calcWingTpaArgument() : throttle; #else const float tpaArgument = throttle; #endif @@ -896,7 +936,7 @@ NOINLINE static void calculateSpaValues(const pidProfile_t *pidProfile) pidRuntime.spa[axis] = 1.0f - smoothStepUpTransition( fabsf(currentRate), pidProfile->spa_center[axis], pidProfile->spa_width[axis]); DEBUG_SET(DEBUG_SPA, axis, lrintf(pidRuntime.spa[axis] * 1000)); - } + } #else UNUSED(pidProfile); #endif // USE_WING @@ -960,7 +1000,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim const bool isExternalAngleModeRequest = FLIGHT_MODE(GPS_RESCUE_MODE) #ifdef USE_ALT_HOLD_MODE - || FLIGHT_MODE(ALT_HOLD_MODE) + || FLIGHT_MODE(ALT_HOLD_MODE) #endif ; levelMode_e levelMode; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9d6e999e1a..6eba82ce0f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -86,6 +86,8 @@ #define TPA_CURVE_PWL_SIZE 17 #endif // USE_ADVANCED_TPA +#define G_ACCELERATION 9.80665f // gravitational acceleration in m/s^2 + typedef enum { TPA_MODE_PD, TPA_MODE_D @@ -161,6 +163,11 @@ typedef enum tpaCurveType_e { TPA_CURVE_HYPERBOLIC, } tpaCurveType_t; +typedef enum tpaSpeedType_e { + TPA_SPEED_BASIC, + TPA_SPEED_ADVANCED, +} tpaSpeedType_t; + #define MAX_PROFILE_NAME_LENGTH 8u typedef struct pidProfile_s { @@ -243,7 +250,7 @@ typedef struct pidProfile_s { uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward uint8_t feedforward_yaw_hold_gain; // Amount of sustained high-pass yaw setpoint to add to feedforward, zero disables uint8_t feedforward_yaw_hold_time ; // Time constant of the sustained yaw hold element in ms to add to feed forward, higher values decay slower - + uint8_t dterm_lpf1_dyn_expo; // set the curve for dynamic dterm lowpass filter uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calculation is gyro based in level mode uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount @@ -278,17 +285,23 @@ typedef struct pidProfile_s { uint8_t ez_landing_speed; // Speed below which motor output is limited uint8_t landing_disarm_threshold; // Accelerometer vector delta (jerk) threshold with disarms if exceeded - uint16_t tpa_delay_ms; // TPA delay for fixed wings using pt2 filter (time constant) uint16_t spa_center[XYZ_AXIS_COUNT]; // RPY setpoint at which PIDs are reduced to 50% (setpoint PID attenuation) uint16_t spa_width[XYZ_AXIS_COUNT]; // Width of smooth transition around spa_center uint8_t spa_mode[XYZ_AXIS_COUNT]; // SPA mode for each axis - uint16_t tpa_gravity_thr0; // For wings: gravity force addition to tpa argument in % when zero throttle - uint16_t tpa_gravity_thr100; // For wings: gravity force addition to tpa argument in % when full throttle uint8_t tpa_curve_type; // Classic type - for multirotor, hyperbolic - usually for wings - uint8_t tpa_curve_stall_throttle; // For wings: speed at which PIDs should be maxed out (stall speed) + uint8_t tpa_curve_stall_throttle; // For wings: speed at which PIDs should be maxed out (stall speed) uint16_t tpa_curve_pid_thr0; // For wings: PIDs multiplier at stall speed uint16_t tpa_curve_pid_thr100; // For wings: PIDs multiplier at full speed int8_t tpa_curve_expo; // For wings: how fast PIDs do transition as speed grows + uint8_t tpa_speed_type; // For wings: relative air speed estimation model type + uint16_t tpa_speed_basic_delay; // For wings when tpa_speed_type = BASIC: delay of air speed estimation from throttle in milliseconds (time of reaching 50% of terminal speed in horizontal flight at full throttle) + uint16_t tpa_speed_basic_gravity; // For wings when tpa_speed_type = BASIC: gravity effect on air speed estimation in percents + uint16_t tpa_speed_adv_prop_pitch; // For wings when tpa_speed_type = ADVANCED: prop pitch in inches * 100 + uint16_t tpa_speed_adv_mass; // For wings when tpa_speed_type = ADVANCED: craft mass in grams + uint16_t tpa_speed_adv_drag_k; // For wings when tpa_speed_type = ADVANCED: craft drag coefficient + uint16_t tpa_speed_adv_thrust; // For wings when tpa_speed_type = ADVANCED: stationary thrust in grams + uint16_t tpa_speed_max_voltage; // For wings: theoretical max voltage; used for throttle scailing with voltage for air speed estimation + int16_t tpa_speed_pitch_offset; // For wings: pitch offset in degrees*10 for craft speed estimation } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -329,6 +342,16 @@ typedef struct pidCoefficient_s { float Kf; } pidCoefficient_t; +typedef struct tpaSpeedParams_s { + float maxSpeed; + float dragMassRatio; + float inversePropMaxSpeed; + float twr; + float speed; + float maxVoltage; + float pitchOffset; +} tpaSpeedParams_t; + typedef struct pidRuntime_s { float dT; float pidFrequency; @@ -460,7 +483,7 @@ typedef struct pidRuntime_s { uint8_t feedforwardMaxRateLimit; float feedforwardYawHoldGain; float feedforwardYawHoldTime; - bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values + bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT]; #endif @@ -475,10 +498,8 @@ typedef struct pidRuntime_s { #endif #ifdef USE_WING - pt2Filter_t tpaLpf; float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation - float tpaGravityThr0; - float tpaGravityThr100; + tpaSpeedParams_t tpaSpeed; #endif // USE_WING #ifdef USE_ADVANCED_TPA diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 28bbcf8c84..fd32265959 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -40,6 +40,8 @@ #include "flight/pid.h" #include "flight/rpm_filter.h" +#include "pg/motor.h" + #include "rx/rx.h" #include "sensors/gyro.h" @@ -66,6 +68,66 @@ static void pidSetTargetLooptime(uint32_t pidLooptime) #endif } +#ifdef USE_WING +void tpaSpeedBasicInit(const pidProfile_t *pidProfile) +{ + // basic model assumes prop pitch speed is inf + const float gravityFactor = pidProfile->tpa_speed_basic_gravity / 100.0f; + const float delaySec = pidProfile->tpa_speed_basic_delay / 1000.0f; + + pidRuntime.tpaSpeed.twr = 1.0f / (gravityFactor * gravityFactor); + const float massDragRatio = (2.0f / logf(3.0f)) * (2.0f / logf(3.0f)) * pidRuntime.tpaSpeed.twr * G_ACCELERATION * delaySec * delaySec; + pidRuntime.tpaSpeed.dragMassRatio = 1.0f / massDragRatio; + pidRuntime.tpaSpeed.maxSpeed = sqrtf(massDragRatio * pidRuntime.tpaSpeed.twr * G_ACCELERATION + G_ACCELERATION); + pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f; +} + +void tpaSpeedAdvancedInit(const pidProfile_t *pidProfile) +{ + // Advanced model uses prop pitch speed, and is quite limited when craft speed is far above prop pitch speed. + pidRuntime.tpaSpeed.twr = (float)pidProfile->tpa_speed_adv_thrust / (float)pidProfile->tpa_speed_adv_mass; + const float mass = pidProfile->tpa_speed_adv_mass / 1000.0f; + const float dragK = pidProfile->tpa_speed_adv_drag_k / 10000.0f; + const float propPitch = pidProfile->tpa_speed_adv_prop_pitch / 100.0f; + pidRuntime.tpaSpeed.dragMassRatio = dragK / mass; + const float propMaxSpeed = (2.54f / 100.0f / 60.0f) * propPitch * motorConfig()->kv * pidRuntime.tpaSpeed.maxVoltage; + if (propMaxSpeed <= 0.0f) { // assuming propMaxSpeed is inf + pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f; + } else { + pidRuntime.tpaSpeed.inversePropMaxSpeed = 1.0f / propMaxSpeed; + } + + const float maxFallSpeed = sqrtf(mass * G_ACCELERATION / dragK); + + const float a = dragK; + const float b = mass * pidRuntime.tpaSpeed.twr * G_ACCELERATION * pidRuntime.tpaSpeed.inversePropMaxSpeed; + const float c = -mass * (pidRuntime.tpaSpeed.twr + 1) * G_ACCELERATION; + + const float maxDiveSpeed = (-b + sqrtf(b*b - 4.0f * a * c)) / (2.0f * a); + + pidRuntime.tpaSpeed.maxSpeed = MAX(maxFallSpeed, maxDiveSpeed); + UNUSED(pidProfile); +} + +void tpaSpeedInit(const pidProfile_t *pidProfile) +{ + pidRuntime.tpaSpeed.speed = 0.0f; + pidRuntime.tpaSpeed.maxVoltage = pidProfile->tpa_speed_max_voltage / 100.0f; + pidRuntime.tpaSpeed.pitchOffset = pidProfile->tpa_speed_pitch_offset * M_PIf / 10.0f / 180.0f; + + switch (pidProfile->tpa_speed_type) { + case TPA_SPEED_BASIC: + tpaSpeedBasicInit(pidProfile); + break; + case TPA_SPEED_ADVANCED: + tpaSpeedAdvancedInit(pidProfile); + break; + default: + break; + } +} +#endif // USE_WING + void pidInitFilters(const pidProfile_t *pidProfile) { STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2 @@ -257,9 +319,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT)); #ifdef USE_WING - pt2FilterInit(&pidRuntime.tpaLpf, pt2FilterGainFromDelay(pidProfile->tpa_delay_ms / 1000.0f, pidRuntime.dT)); - pidRuntime.tpaGravityThr0 = pidProfile->tpa_gravity_thr0 / 100.0f; - pidRuntime.tpaGravityThr100 = pidProfile->tpa_gravity_thr100 / 100.0f; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pidRuntime.spa[axis] = 1.0f; // 1.0 = no PID attenuation in runtime. 0 - full attenuation (no PIDs) } @@ -496,6 +555,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidRuntime.useEzDisarm = pidProfile->landing_disarm_threshold > 0; pidRuntime.landingDisarmThreshold = pidProfile->landing_disarm_threshold * 10.0f; +#ifdef USE_WING + tpaSpeedInit(pidProfile); +#endif } void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)