diff --git a/src/board.h b/src/board.h index 071c0262..a81b6615 100755 --- a/src/board.h +++ b/src/board.h @@ -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 { diff --git a/src/cli.c b/src/cli.c index 38a4bb8f..09779c60 100644 --- a/src/cli.c +++ b/src/cli.c @@ -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 }; diff --git a/src/mixer.c b/src/mixer.c index 16d35032..520af8e9 100644 --- a/src/mixer.c +++ b/src/mixer.c @@ -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) { @@ -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(); } @@ -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]); @@ -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]); } @@ -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]; @@ -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 @@ -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); } } @@ -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; @@ -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)