mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 14:55:21 +03:00
Changed direction parameters into boolean to ensure a direction is defined.
This commit is contained in:
parent
380fa0e90e
commit
bed980bb27
10 changed files with 14 additions and 13 deletions
|
@ -40,3 +40,5 @@ typedef enum {
|
||||||
} angle_index_t;
|
} angle_index_t;
|
||||||
|
|
||||||
#define ANGLE_INDEX_COUNT 2
|
#define ANGLE_INDEX_COUNT 2
|
||||||
|
|
||||||
|
#define GET_DIRECTION(isInverted) (2 * (isInverted) - 1)
|
||||||
|
|
|
@ -655,7 +655,7 @@ static const clivalue_t valueTable[] = {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// PG_MIXER_CONFIG
|
// 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
|
// 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
|
{ "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
|
#endif
|
||||||
|
|
||||||
// PG_AIRPLANE_CONFIG
|
// 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
|
// 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) },
|
{ "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) },
|
||||||
|
|
|
@ -642,7 +642,7 @@ void createDefaultConfig(master_t *config)
|
||||||
config->armingConfig.disarm_kill_switch = 1;
|
config->armingConfig.disarm_kill_switch = 1;
|
||||||
config->armingConfig.auto_disarm_delay = 5;
|
config->armingConfig.auto_disarm_delay = 5;
|
||||||
|
|
||||||
config->airplaneConfig.fixedwing_althold_dir = 1;
|
config->airplaneConfig.fixedwing_althold_reversed = false;
|
||||||
|
|
||||||
// Motor/ESC/Servo
|
// Motor/ESC/Servo
|
||||||
resetMixerConfig(&config->mixerConfig);
|
resetMixerConfig(&config->mixerConfig);
|
||||||
|
|
|
@ -273,7 +273,7 @@ void updateMagHold(void)
|
||||||
dif += 360;
|
dif += 360;
|
||||||
if (dif >= +180)
|
if (dif >= +180)
|
||||||
dif -= 360;
|
dif -= 360;
|
||||||
dif *= -GET_YAW_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
||||||
if (STATE(SMALL_ANGLE))
|
if (STATE(SMALL_ANGLE))
|
||||||
rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30; // 18 deg
|
rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30; // 18 deg
|
||||||
} else
|
} else
|
||||||
|
|
|
@ -284,7 +284,7 @@ void updateRcCommands(void)
|
||||||
} else {
|
} else {
|
||||||
tmp = 0;
|
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) {
|
if (rcData[axis] < rxConfig()->midrc) {
|
||||||
rcCommand[axis] = -rcCommand[axis];
|
rcCommand[axis] = -rcCommand[axis];
|
||||||
|
|
|
@ -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 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_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
|
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;
|
} rcControlsConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
|
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
|
||||||
|
|
||||||
#define GET_YAW_DIRECTION(isInverted) (2 * (isInverted) - 1)
|
|
||||||
typedef struct flight3DConfig_s {
|
typedef struct flight3DConfig_s {
|
||||||
uint16_t deadband3d_low; // min 3d value
|
uint16_t deadband3d_low; // min 3d value
|
||||||
uint16_t deadband3d_high; // max 3d value
|
uint16_t deadband3d_high; // max 3d value
|
||||||
|
|
|
@ -47,7 +47,7 @@
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
|
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
|
||||||
.fixedwing_althold_dir = 1
|
.fixedwing_althold_reversed = false
|
||||||
);
|
);
|
||||||
|
|
||||||
int32_t setVelocity = 0;
|
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
|
// most likely need to check changes on pitch channel and 'reset' althold similar to
|
||||||
// how throttle does it on multirotor
|
// 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)
|
void applyAltHold(void)
|
||||||
|
|
|
@ -23,7 +23,7 @@ extern int32_t AltHold;
|
||||||
extern int32_t vario;
|
extern int32_t vario;
|
||||||
|
|
||||||
typedef struct airplaneConfig_s {
|
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;
|
} airplaneConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(airplaneConfig_t, airplaneConfig);
|
PG_DECLARE(airplaneConfig_t, airplaneConfig);
|
||||||
|
|
|
@ -67,7 +67,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
|
||||||
#endif
|
#endif
|
||||||
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
||||||
.mixerMode = TARGET_DEFAULT_MIXER,
|
.mixerMode = TARGET_DEFAULT_MIXER,
|
||||||
.yaw_motor_direction = 1,
|
.yaw_motors_reversed = false,
|
||||||
);
|
);
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
|
||||||
|
@ -530,7 +530,7 @@ void mixTable(pidProfile_t *pidProfile)
|
||||||
motorMix[i] =
|
motorMix[i] =
|
||||||
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
|
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
|
||||||
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
|
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) {
|
if (vbatCompensationFactor > 1.0f) {
|
||||||
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
|
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
|
||||||
|
|
|
@ -100,7 +100,7 @@ typedef struct mixer_s {
|
||||||
|
|
||||||
typedef struct mixerConfig_s {
|
typedef struct mixerConfig_s {
|
||||||
uint8_t mixerMode;
|
uint8_t mixerMode;
|
||||||
int8_t yaw_motor_direction;
|
bool yaw_motors_reversed;
|
||||||
} mixerConfig_t;
|
} mixerConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(mixerConfig_t, mixerConfig);
|
PG_DECLARE(mixerConfig_t, mixerConfig);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue