diff --git a/src/main/common/axis.h b/src/main/common/axis.h index 387d96c265..7b20dada4a 100644 --- a/src/main/common/axis.h +++ b/src/main/common/axis.h @@ -40,3 +40,5 @@ typedef enum { } angle_index_t; #define ANGLE_INDEX_COUNT 2 + +#define GET_DIRECTION(isInverted) (2 * (isInverted) - 1) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 2e32aed022..f5a32df9e2 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -655,7 +655,7 @@ static const clivalue_t valueTable[] = { #endif // PG_MIXER_CONFIG - { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motor_direction) }, + { "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) }, // PG_MOTOR_3D_CONFIG { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, // FIXME upper limit should match code in the mixer, 1500 currently @@ -721,7 +721,7 @@ static const clivalue_t valueTable[] = { #endif // PG_AIRPLANE_CONFIG - { "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, .config.minmax = { -1, 1 }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_dir) }, + { "fixedwing_althold_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_reversed) }, // PG_RC_CONTROLS_CONFIG { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e8b12e519f..ac222cfebe 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -642,7 +642,7 @@ void createDefaultConfig(master_t *config) config->armingConfig.disarm_kill_switch = 1; config->armingConfig.auto_disarm_delay = 5; - config->airplaneConfig.fixedwing_althold_dir = 1; + config->airplaneConfig.fixedwing_althold_reversed = false; // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 8e35f412ee..665114d4a8 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -273,7 +273,7 @@ void updateMagHold(void) dif += 360; if (dif >= +180) dif -= 360; - dif *= -GET_YAW_DIRECTION(rcControlsConfig()->yaw_control_reversed); + dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); if (STATE(SMALL_ANGLE)) rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30; // 18 deg } else diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index ab6a6c610c..04647546de 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -284,7 +284,7 @@ void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = tmp * -GET_YAW_DIRECTION(rcControlsConfig()->yaw_control_reversed); + rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); } if (rcData[axis] < rxConfig()->midrc) { rcCommand[axis] = -rcCommand[axis]; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 5fc1c4ca44..c96502dc1f 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -158,12 +158,11 @@ typedef struct rcControlsConfig_s { uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement - int8_t yaw_control_reversed; // invert control direction of yaw + bool yaw_control_reversed; // invert control direction of yaw } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); -#define GET_YAW_DIRECTION(isInverted) (2 * (isInverted) - 1) typedef struct flight3DConfig_s { uint16_t deadband3d_low; // min 3d value uint16_t deadband3d_high; // max 3d value diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index c6bd68aa14..7797bc4986 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -47,7 +47,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, - .fixedwing_althold_dir = 1 + .fixedwing_althold_reversed = false ); int32_t setVelocity = 0; @@ -114,7 +114,7 @@ static void applyFixedWingAltHold(void) // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir; + rcCommand[PITCH] += altHoldThrottleAdjustment * GET_DIRECTION(airplaneConfig()->fixedwing_althold_reversed); } void applyAltHold(void) diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index f1275a50e0..14e48d2052 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -23,7 +23,7 @@ extern int32_t AltHold; extern int32_t vario; typedef struct airplaneConfig_s { - int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign + bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign } airplaneConfig_t; PG_DECLARE(airplaneConfig_t, airplaneConfig); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b5b5a28c14..aeefaa2307 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -67,7 +67,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #endif PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = TARGET_DEFAULT_MIXER, - .yaw_motor_direction = 1, + .yaw_motors_reversed = false, ); PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); @@ -530,7 +530,7 @@ void mixTable(pidProfile_t *pidProfile) motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction); + scaledAxisPIDf[YAW] * currentMixer[i].yaw * -GET_DIRECTION(mixerConfig()->yaw_motors_reversed); if (vbatCompensationFactor > 1.0f) { motorMix[i] *= vbatCompensationFactor; // Add voltage compensation diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1cfc0620e6..5f88ff3aeb 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -100,7 +100,7 @@ typedef struct mixer_s { typedef struct mixerConfig_s { uint8_t mixerMode; - int8_t yaw_motor_direction; + bool yaw_motors_reversed; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig);