From 73a280cc85e2cd0526f3bf759925d391344bf225 Mon Sep 17 00:00:00 2001 From: Tdogb Date: Mon, 16 Dec 2019 17:24:55 -0500 Subject: [PATCH 1/3] initial commit of crashflip motor power percentage control --- src/main/cli/settings.c | 1 + src/main/flight/mixer.c | 3 ++- src/main/flight/mixer.h | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f0759986cd..43945d89c4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -873,6 +873,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) }, { "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) }, + { "crashflip_power_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_power_percent) }, // PG_MOTOR_3D_CONFIG { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 918a3eb0b9..3f1fc1aa72 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -74,6 +74,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = DEFAULT_MIXER, .yaw_motors_reversed = false, .crashflip_motor_percent = 0, + .crashflip_power_percent = 100, ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); @@ -662,7 +663,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) // Apply a reasonable amount of stick deadband const float flipStickRange = 1.0f - CRASH_FLIP_STICK_MINF; - float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / flipStickRange; + float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / (flipStickRange / ((float)mixerConfig()->crashflip_power_percent / 100.0f)); for (int i = 0; i < motorCount; ++i) { float motorOutputNormalised = diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 01b0230cc1..5fc13218bc 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -81,6 +81,7 @@ typedef struct mixerConfig_s { uint8_t mixerMode; bool yaw_motors_reversed; uint8_t crashflip_motor_percent; + uint8_t crashflip_power_percent; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); From 3243aafdb952a05ed735e0f028197c36203d10e2 Mon Sep 17 00:00:00 2001 From: Tdogb Date: Tue, 17 Dec 2019 12:57:47 -0500 Subject: [PATCH 2/3] crashflip expo --- src/main/cli/settings.c | 2 +- src/main/flight/mixer.c | 25 +++++++++++++++++-------- src/main/flight/mixer.h | 2 +- 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 43945d89c4..1ed8b742e7 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -873,7 +873,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) }, { "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) }, - { "crashflip_power_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_power_percent) }, + { "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) }, // PG_MOTOR_3D_CONFIG { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3f1fc1aa72..2f19f44aa9 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -74,7 +74,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = DEFAULT_MIXER, .yaw_motors_reversed = false, .crashflip_motor_percent = 0, - .crashflip_power_percent = 100, + .crashflip_expo = 0, ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); @@ -630,18 +630,26 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static void applyFlipOverAfterCrashModeToMotors(void) { if (ARMING_FLAG(ARMED)) { - float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH); - float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL); - float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW); + const float flipPowerFactor = 1.0f - mixerConfig()->crashflip_expo / 100.0f; + const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH); + const float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL); + const float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW); + + const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor); + const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor); + const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor); + float signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1; float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1; float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1); - float stickDeflectionLength = sqrtf(stickDeflectionPitchAbs*stickDeflectionPitchAbs + stickDeflectionRollAbs*stickDeflectionRollAbs); + float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); + float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { // If yaw is the dominant, disable pitch and roll stickDeflectionLength = stickDeflectionYawAbs; + stickDeflectionExpoLength = stickDeflectionYawExpo; signRoll = 0; signPitch = 0; } else { @@ -649,7 +657,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) signYaw = 0; } - float cosPhi = (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength); + const float cosPhi = (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength); const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f) if (cosPhi < cosThreshold) { @@ -662,8 +670,9 @@ static void applyFlipOverAfterCrashModeToMotors(void) } // Apply a reasonable amount of stick deadband - const float flipStickRange = 1.0f - CRASH_FLIP_STICK_MINF; - float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / (flipStickRange / ((float)mixerConfig()->crashflip_power_percent / 100.0f)); + const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor); + const float flipStickRange = 1.0f - crashFlipStickMinExpo; + const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; for (int i = 0; i < motorCount; ++i) { float motorOutputNormalised = diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5fc13218bc..331d4dbe82 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -81,7 +81,7 @@ typedef struct mixerConfig_s { uint8_t mixerMode; bool yaw_motors_reversed; uint8_t crashflip_motor_percent; - uint8_t crashflip_power_percent; + uint8_t crashflip_expo; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); From 91333f37818b213c3252ba01214e9d72399c7907 Mon Sep 17 00:00:00 2001 From: Tdogb Date: Sun, 12 Jan 2020 17:01:13 -0500 Subject: [PATCH 3/3] updated default crashflip expo to 35 --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2f19f44aa9..5e3e62a130 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -74,7 +74,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = DEFAULT_MIXER, .yaw_motors_reversed = false, .crashflip_motor_percent = 0, - .crashflip_expo = 0, + .crashflip_expo = 35 ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);