Merge pull request #2754 from mikeller/make_direction_parameters_boolean

Changed direction parameters into boolean to ensure a direction is defined.
This commit is contained in:
Michael Keller 2017-03-28 23:08:27 +13:00 committed by GitHub
commit c059c9f64d
10 changed files with 14 additions and 13 deletions

View File

@ -40,3 +40,5 @@ typedef enum {
} angle_index_t;
#define ANGLE_INDEX_COUNT 2
#define GET_DIRECTION(isInverted) (2 * (isInverted) - 1)

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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