Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Revert "Make servoMixer work for types other than CUSTOM_PLANE" #298

Merged
merged 1 commit into from
Sep 9, 2015
Merged

Revert "Make servoMixer work for types other than CUSTOM_PLANE" #298

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion src/board.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,6 @@ typedef enum {
FEATURE_FW_FAILSAFE_RTH = 1 << 15,
FEATURE_SYNCPWM = 1 << 16,
FEATURE_FASTPWM = 1 << 17,
FEATURE_SERVO_MIXER = 1 << 18,
} AvailableFeatures;

typedef enum {
Expand Down
2 changes: 1 addition & 1 deletion src/cli.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ static const char *const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP",
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
"FW_FAILSAFE_RTH", "SYNCPWM", "FASTPWM", "SERVO_MIXER",
"FW_FAILSAFE_RTH", "SYNCPWM", "FASTPWM",
NULL
};

Expand Down
112 changes: 50 additions & 62 deletions src/mixer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -283,8 +283,8 @@ void mixerInit(void)

// enable servos for mixes that require them. note, this shifts motor counts.
core.useServo = mixers[mcfg.mixerConfiguration].useServo;
// if we want camstab/trig or servo mixers, that also enables servos, even if mixerConfiguration doesn't
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_SERVO_MIXER))
// if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT))
core.useServo = 1;

if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) {
Expand Down Expand Up @@ -327,12 +327,12 @@ void mixerInit(void)
// set flag that we're on something with wings
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE) {
f.FIXED_WING = 1;

if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE)
loadCustomServoMixer();
} else
f.FIXED_WING = 0;

if (core.useServo)
loadCustomServoMixer();

mixerResetMotors();
}

Expand Down Expand Up @@ -382,17 +382,6 @@ void writeServos(void)
if (!core.useServo)
return;

// forward AUX1-4 to servo outputs (not constrained)
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = core.numServos - 4;
// offset servos based off number already used in mixer types
// airplane and servo_tilt together can't be used
// calculate offset by taking 4 from core.numServos
for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]);
}

// apply servos for the specific mixerConfiguration
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
pwmWriteServo(0, servo[4]);
Expand Down Expand Up @@ -449,8 +438,8 @@ void writeServos(void)
break;

default:
// otherwise, control the first two servos when SERVO_TILT or SERVO_MIXER is enabled
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_SERVO_MIXER)) {
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
pwmWriteServo(0, servo[0]);
pwmWriteServo(1, servo[1]);
}
Expand All @@ -476,15 +465,6 @@ void writeAllMotors(int16_t mc)
writeMotors();
}

static void resetServos(void)
{
int i;

// reset all servos to their middle value
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = servoMiddle(i);
}

static void servoMixer(void)
{
int16_t input[INPUT_ITEMS];
Expand Down Expand Up @@ -520,6 +500,9 @@ static void servoMixer(void)
input[INPUT_RC_YAW] = rcData[YAW] - mcfg.midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - mcfg.midrc;

for (i = 0; i < MAX_SERVOS; i++)
servo[i] = 0;

// mix servos according to rules
for (i = 0; i < numberRules; i++) {
// consider rule if no box assigned or box is active
Expand All @@ -530,35 +513,24 @@ static void servoMixer(void)
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;

if (currentServoMixer[i].speed == 0) {
// directly use the input value if speed is not provided
if (currentServoMixer[i].speed == 0)
currentOutput[i] = input[from];
} else {
// apply speed constraints
else {
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}

// start with the output value
servo[target] = (int16_t)currentOutput[i];

// apply rate from mixer rule
servo[target] *= ((float)currentServoMixer[i].rate / 100);

// apply rate fro m servoconfiguration
servo[target] *= ((float)cfg.servoConf[target].rate / 100);

// constrain the width of the servo's movement
servo[target] = constrain(servo[target], min, max);

// reverse direction if necessary
servo[target] *= servoDirection(target, from);
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else
currentOutput[i] = 0;
}

// center the servo around its middle
servo[target] += servoMiddle(i);
}
// servo rates
for (i = 0; i < MAX_SERVOS; i++) {
servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100;
servo[i] += servoMiddle(i);
}
}

Expand All @@ -584,19 +556,29 @@ void mixTable(void)
motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
}

// reset all servos
if (core.useServo)
resetServos();

if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) {
// set servo output for gimbal type
servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
// airplane / servo mixes
switch (mcfg.mixerConfiguration) {
case MULTITYPE_CUSTOM_PLANE:
case MULTITYPE_FLYING_WING:
case MULTITYPE_AIRPLANE:
case MULTITYPE_BI:
case MULTITYPE_TRI:
case MULTITYPE_DUALCOPTER:
case MULTITYPE_SINGLECOPTER:
servoMixer();
break;
case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
break;
}

// set camstab servo output before applying servo mixer rules
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
// vary either pitch or roll by RC channel
// center at fixed position, or vary either pitch or roll by RC channel
servo[0] = servoMiddle(0);
servo[1] = servoMiddle(1);

if (rcOptions[BOXCAMSTAB]) {
if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
Expand All @@ -608,14 +590,20 @@ void mixTable(void)
}
}

// run the servo mixer if necessary
if (core.useServo)
servoMixer();

// constrain servos
for (i = 0; i < MAX_SERVOS; i++)
servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values

// forward AUX1-4 to servo outputs (not constrained)
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = core.numServos - 4;
// offset servos based off number already used in mixer types
// airplane and servo_tilt together can't be used
// calculate offset by taking 4 from core.numServos
for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]);
}

maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
Expand Down