Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

crashflip expo

Browse source
This commit is contained in:
Tdogb 2019-12-17 12:57:47 -05:00
parent 73a280cc85
commit 3243aafdb9
3 changed files with 19 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -873,7 +873,7 @@ const clivalue_t valueTable[] = {
// PG_MIXER_CONFIG // 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) }, { "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_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 // 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) }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) },

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

@ -74,7 +74,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.mixerMode = DEFAULT_MIXER, .mixerMode = DEFAULT_MIXER,
.yaw_motors_reversed = false, .yaw_motors_reversed = false,
.crashflip_motor_percent = 0, .crashflip_motor_percent = 0,
.crashflip_power_percent = 100, .crashflip_expo = 0,
); );
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 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) static void applyFlipOverAfterCrashModeToMotors(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH); const float flipPowerFactor = 1.0f - mixerConfig()->crashflip_expo / 100.0f;
float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL); const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH);
float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW); 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 signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1;
float signRoll = getRcDeflection(FD_ROLL) < 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 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 (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll // If yaw is the dominant, disable pitch and roll
stickDeflectionLength = stickDeflectionYawAbs; stickDeflectionLength = stickDeflectionYawAbs;
stickDeflectionExpoLength = stickDeflectionYawExpo;
signRoll = 0; signRoll = 0;
signPitch = 0; signPitch = 0;
} else { } else {
@ -649,7 +657,7 @@ static void applyFlipOverAfterCrashModeToMotors(void)
signYaw = 0; 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) const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
if (cosPhi < cosThreshold) { if (cosPhi < cosThreshold) {
@ -662,8 +670,9 @@ static void applyFlipOverAfterCrashModeToMotors(void)
} }
// Apply a reasonable amount of stick deadband // Apply a reasonable amount of stick deadband
const float flipStickRange = 1.0f - CRASH_FLIP_STICK_MINF; const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor);
float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / (flipStickRange / ((float)mixerConfig()->crashflip_power_percent / 100.0f)); const float flipStickRange = 1.0f - crashFlipStickMinExpo;
const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
for (int i = 0; i < motorCount; ++i) { for (int i = 0; i < motorCount; ++i) {
float motorOutputNormalised = float motorOutputNormalised =

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

@ -81,7 +81,7 @@ typedef struct mixerConfig_s {
uint8_t mixerMode; uint8_t mixerMode;
bool yaw_motors_reversed; bool yaw_motors_reversed;
uint8_t crashflip_motor_percent; uint8_t crashflip_motor_percent;
uint8_t crashflip_power_percent; uint8_t crashflip_expo;
} mixerConfig_t; } mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig); PG_DECLARE(mixerConfig_t, mixerConfig);