diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 16b2a68f63..0ef8467cdf 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -45,6 +45,8 @@ #include "config/config.h" #include "config/feature.h" +#include "cli/settings.h" + #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -1381,7 +1383,7 @@ static bool blackboxWriteSysinfo(void) } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } - ); + ); BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatwarningcellvoltage, @@ -1392,7 +1394,7 @@ static bool blackboxWriteSysinfo(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { blackboxPrintfHeaderLine("currentSensor", "%d,%d",currentSensorADCConfig()->offset, currentSensorADCConfig()->scale); } - ); + ); BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.sampleLooptime); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", 1); @@ -1402,9 +1404,13 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d", currentPidProfile->tpa_mode); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_RATE, "%d", currentPidProfile->tpa_low_rate); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d", currentPidProfile->tpa_low_breakpoint); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d", currentPidProfile->tpa_low_always); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_RATE, "%d", currentPidProfile->tpa_low_rate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d", currentPidProfile->tpa_low_breakpoint); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d", currentPidProfile->tpa_low_always); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MIXER_TYPE, "%s", lookupTableMixerType[mixerConfig()->mixer_type]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_THRESHOLD, "%d", currentPidProfile->ez_landing_threshold); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_LIMIT, "%d", currentPidProfile->ez_landing_limit); + BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL], currentControlRateProfile->rcRates[PITCH], currentControlRateProfile->rcRates[YAW]); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 0d32ec78a3..30bd2d7c2d 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RC_STATS", "MAG_CALIB", "MAG_TASK_RATE", + "EZLANDING", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 21a7e52cd5..ab4c0169ae 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -118,6 +118,7 @@ typedef enum { DEBUG_RC_STATS, DEBUG_MAG_CALIB, DEBUG_MAG_TASK_RATE, + DEBUG_EZLANDING, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index c9b188c61f..22d3490aa8 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -501,8 +501,8 @@ const char * const lookupTableSimplifiedTuningPidsMode[] = { "OFF", "RP", "RPY", }; -static const char* const lookupTableMixerType[] = { - "LEGACY", "LINEAR", "DYNAMIC", +const char* const lookupTableMixerType[] = { + "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING", }; #ifdef USE_OSD @@ -945,7 +945,7 @@ const clivalue_t valueTable[] = { // PG_MIXER_CONFIG { "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) }, - { "mixer_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) }, + { PARAM_NAME_MIXER_TYPE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) }, { "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) }, { "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) }, #ifdef USE_RPM_LIMIT @@ -1250,7 +1250,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter_multiplier) }, #endif #ifdef USE_TPA_MODE - { PARAM_NAME_TPA_MODE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_mode) }, + { PARAM_NAME_TPA_MODE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_mode) }, #endif { PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate) }, { PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) }, @@ -1258,6 +1258,9 @@ const clivalue_t valueTable[] = { { PARAM_NAME_TPA_LOW_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_breakpoint) }, { 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) }, + { PARAM_NAME_EZ_LANDING_THRESHOLD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) }, + { PARAM_NAME_EZ_LANDING_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 75 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_limit) }, + // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 27c4ea5678..e4c6abf69d 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -269,6 +269,8 @@ extern const char * const lookupTableOffOn[]; extern const char * const lookupTableSimplifiedTuningPidsMode[]; +extern const char * const lookupTableMixerType[]; + extern const char * const lookupTableCMSMenuBackgroundType[]; extern const char * const lookupTableThrottleLimitType[]; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 448ed153c3..55bd8592e9 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -58,6 +58,9 @@ #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_MIXER_TYPE "mixer_type" +#define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" +#define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" #define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type" #define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent" #define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index a523d89d68..0cf640b480 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -60,7 +60,10 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom // note that rcCommand[] is an external float static float rawSetpoint[XYZ_AXIS_COUNT]; + static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; // deflection range -1 to 1 +static float maxRcDeflectionAbs; + static bool reverseMotors = false; static applyRatesFn *applyRates; @@ -140,6 +143,11 @@ float getRcDeflectionAbs(int axis) return rcDeflectionAbs[axis]; } +float getMaxRcDeflectionAbs(void) +{ + return maxRcDeflectionAbs; +} + #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE @@ -620,6 +628,7 @@ FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, int axis) FAST_CODE void processRcCommand(void) { if (isRxDataNew) { + maxRcDeflectionAbs = 0.0f; for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { float angleRate; @@ -646,6 +655,7 @@ FAST_CODE void processRcCommand(void) rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = fabsf(rcCommandf); rcDeflectionAbs[axis] = rcCommandfAbs; + maxRcDeflectionAbs = fmaxf(maxRcDeflectionAbs, rcCommandfAbs); angleRate = applyRates(axis, rcCommandf, rcCommandfAbs); } diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index 4b5e5c5e46..34911c7a6d 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -34,6 +34,7 @@ float getSetpointRate(int axis); float getRcDeflection(int axis); float getRcDeflectionRaw(int axis); float getRcDeflectionAbs(int axis); +float getMaxRcDeflectionAbs(void); void updateRcCommands(void); void resetYawAxis(void); void initRcProcessing(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9f3ba386dc..524b2de081 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -255,7 +255,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #else motorRangeMax = mixerRuntime.motorOutputHigh; #endif - motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); motorOutputMin = motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin; @@ -423,6 +422,8 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t motor[i] = motor_disarmed[i]; } } + DEBUG_SET(DEBUG_EZLANDING, 1, throttle * 10000U); + // DEBUG_EZLANDING 0 is the ezLanding factor 2 is the throttle limit } static float applyThrottleLimit(float throttle) @@ -502,6 +503,60 @@ static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnable throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor); } +static float calcEzLandLimit(float maxDeflection) +{ + // calculate limit to where the mixer can raise the throttle based on RPY stick deflection + // 0.0 = no increas allowed, 1.0 = 100% increase allowed + float ezLandLimit = 1.0f; + if (maxDeflection < mixerRuntime.ezLandingThreshold) { // roll, pitch and yaw sticks under threshold + ezLandLimit = maxDeflection / mixerRuntime.ezLandingThreshold; // normalised 0 - 1 + ezLandLimit = fmaxf(ezLandLimit, mixerRuntime.ezLandingLimit); // stay above the minimum + } + return ezLandLimit; +} + +static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin, const float motorMixMax) +{ + // Calculate factor for normalizing motor mix range to <= 1.0 + const float baseNormalizationFactor = motorMixRange > 1.0f ? 1.0f / motorMixRange : 1.0f; + const float normalizedMotorMixMin = motorMixMin * baseNormalizationFactor; + const float normalizedMotorMixMax = motorMixMax * baseNormalizationFactor; + + // Upper throttle limit + // range default 0.05 - 1.0 with ezLandingLimit = 5, no stick deflection -> 0.05 + const float ezLandLimit = calcEzLandLimit(getMaxRcDeflectionAbs()); + // use the largest of throttle and limit calculated from RPY stick positions + float upperLimit = fmaxf(ezLandLimit, throttle); + // limit throttle to avoid clipping the highest motor output + upperLimit = fminf(upperLimit, 1.0f - normalizedMotorMixMax); + + // Lower throttle Limit + const float epsilon = 1.0e-6f; // add small value to avoid divisions by zero + const float absMotorMixMin = fabsf(normalizedMotorMixMin) + epsilon; + const float lowerLimit = fminf(upperLimit, absMotorMixMin); + + // represents how much motor values have to be scaled to avoid clipping + const float ezLandFactor = upperLimit / absMotorMixMin; + + // scale motor values + const float normalizationFactor = baseNormalizationFactor * fminf(1.0f, ezLandFactor); + for (int i = 0; i < mixerRuntime.motorCount; i++) { + motorMix[i] *= normalizationFactor; + } + motorMixRange *= baseNormalizationFactor; + // Make anti windup recognize reduced authority range + motorMixRange = fmaxf(motorMixRange, 1.0f / ezLandFactor); + + // Constrain throttle + throttle = constrainf(throttle, lowerLimit, upperLimit); + + // Log ezLandFactor, upper throttle limit, and ezLandFactor if throttle was zero + DEBUG_SET(DEBUG_EZLANDING, 0, fminf(1.0f, ezLandFactor) * 10000U); + // DEBUG_EZLANDING 1 is the adjusted throttle + DEBUG_SET(DEBUG_EZLANDING, 2, upperLimit * 10000U); + DEBUG_SET(DEBUG_EZLANDING, 3, fminf(1.0f, ezLandLimit / absMotorMixMin) * 10000U); +} + static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled) { #ifdef USE_AIRMODE_LPF @@ -665,10 +720,21 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) #endif motorMixRange = motorMixMax - motorMixMin; - if (mixerConfig()->mixer_type > MIXER_LEGACY) { - applyMixerAdjustmentLinear(motorMix, airmodeEnabled); - } else { + + switch (mixerConfig()->mixer_type) { + case MIXER_LEGACY: applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled); + break; + case MIXER_LINEAR: + case MIXER_DYNAMIC: + applyMixerAdjustmentLinear(motorMix, airmodeEnabled); + break; + case MIXER_EZLANDING: + applyMixerAdjustmentEzLand(motorMix, motorMixMin, motorMixMax); + break; + default: + applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled); + break; } if (featureIsEnabled(FEATURE_MOTOR_STOP) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index c81cbe004a..74dd334c6b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -71,6 +71,7 @@ typedef enum mixerType MIXER_LEGACY = 0, MIXER_LINEAR = 1, MIXER_DYNAMIC = 2, + MIXER_EZLANDING = 3, } mixerType_e; // Custom mixer data per motor diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 50d7c9edfb..a1edc86eea 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -365,6 +365,9 @@ void mixerInitProfile(void) pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.0f, pidGetDT())); mixerResetRpmLimiter(); #endif + + mixerRuntime.ezLandingThreshold = 2.0f * currentPidProfile->ez_landing_threshold / 100.0f; + mixerRuntime.ezLandingLimit = currentPidProfile->ez_landing_limit / 100.0f; } #ifdef USE_RPM_LIMIT diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h index 098ddc2e06..1d2c557772 100644 --- a/src/main/flight/mixer_init.h +++ b/src/main/flight/mixer_init.h @@ -63,6 +63,8 @@ typedef struct mixerRuntime_s { pt1Filter_t rpmLimiterAverageRpmFilter; pt1Filter_t rpmLimiterThrottleScaleOffsetFilter; #endif + float ezLandingThreshold; + float ezLandingLimit; } mixerRuntime_t; extern mixerRuntime_t mixerRuntime; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a863b57e30..89904ab336 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -227,6 +227,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .tpa_low_rate = 20, .tpa_low_breakpoint = 1050, .tpa_low_always = 0, + .ez_landing_threshold = 25, + .ez_landing_limit = 5, ); #ifndef USE_D_MIN diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6e659efb84..70e4277e1d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -234,12 +234,16 @@ typedef struct pidProfile_s { uint8_t tpa_mode; // Controls which PID terms TPA effects uint8_t tpa_rate; // Percent reduction in P or D at full throttle uint16_t tpa_breakpoint; // Breakpoint where TPA is activated + uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds - uint8_t angle_earth_ref; // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode - uint16_t horizon_delay_ms; // delay when Horizon Strength increases, 50 = 500ms time constant - uint8_t tpa_low_rate; // Percent reduction in P or D at zero throttle - uint16_t tpa_low_breakpoint; // Breakpoint where lower TPA is deactivated - uint8_t tpa_low_always; // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time + uint8_t angle_earth_ref; // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode + uint16_t horizon_delay_ms; // delay when Horizon Strength increases, 50 = 500ms time constant + uint8_t tpa_low_rate; // Percent reduction in P or D at zero throttle + uint16_t tpa_low_breakpoint; // Breakpoint where lower TPA is deactivated + uint8_t tpa_low_always; // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time + + uint8_t ez_landing_threshold; // Threshold stick position below which motor output is limited + uint8_t ez_landing_limit; // Maximum motor output when all sticks centred and throttle zero } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);