Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Throttlebased EzLanding (#12094)

Browse source 
* ezLanding

* Add ez_landing throttle mode

* Correct EzLanding scaling of motorMixRange

* Correct mixer_type switch bracing style

* Remove motor value cliping ez landing mode

- rename mixer type cli setting to EZLANDING from EZLANDING_THROTTLE
- remove EZLANDING_CLIP cli setting
- double default ez_landing_threshold
- halve default ez_landing_limit
- check  and  limits in cli settings
- remove mixer type dependent settings in mixer_init
- remove clip based code in mixer.c

* Change ez_landing setting values and refactoring

- Halve defaul ez_landing_threshold setting and double in init instead.
  Now stick deflection equal to ez_landing_threshold should give approimately full authority.
  Previously it was the point where the mixer was allowed to raise the throttle to 100 % (which wouuld never be required)
- Increase ez_landing_threshold maximum to 200 (from 100) to allow settings that increase authority by a little at full stick deflection
- Increase ez_landing_limit maximum to 75 which is the point where EzLanding should act identical to the Legacy mixer with airmode on
- remove throttle percent from
- simplify calculation of , since throttle stick deflection is no longer involved
- update/remove outdated comments

* Remove old EZLANDING entries in mixerType enum

* Add mixer_type setting to blackbox log header

---------

Co-authored-by: ctzsnooze <chris.thompson@sydney.edu.au>
Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
This commit is contained in:
tbolin 2023-12-09 02:36:41 +01:00 • committed by GitHub
parent d3830b6b34
commit ae71256da7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 123 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

16
src/main/blackbox/blackbox.c
View file

@ -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]);

1
src/main/build/debug.c
View file

@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RC_STATS",
"MAG_CALIB",
"MAG_TASK_RATE",
"EZLANDING",
};

1
src/main/build/debug.h
View file

@ -118,6 +118,7 @@ typedef enum {
DEBUG_RC_STATS,
DEBUG_MAG_CALIB,
DEBUG_MAG_TASK_RATE,
DEBUG_EZLANDING,
DEBUG_COUNT
} debugType_e;

11
src/main/cli/settings.c
View file

@ -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) },

2
src/main/cli/settings.h
View file

@ -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[];

3
src/main/fc/parameter_names.h
View file

@ -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"

10
src/main/fc/rc.c
View file

@ -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);
}

1
src/main/fc/rc.h
View file

@ -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);

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

@ -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)

1
src/main/flight/mixer.h
View file

@ -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

3
src/main/flight/mixer_init.c
View file

@ -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

2
src/main/flight/mixer_init.h
View file

@ -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;

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

@ -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

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

@ -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);