Merge pull request #289 from lazd/servomixerfeat

Make servoMixer work for types other than CUSTOM_PLANE
multiwii · Jul 15, 2015 · ba9c0d2 · ba9c0d2
2 parents 63bacbc + 4a84151
commit ba9c0d2
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 51 deletions.
diff --git a/src/board.h b/src/board.h
@@ -99,6 +99,7 @@ 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
@@ -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",
+    "FW_FAILSAFE_RTH", "SYNCPWM", "FASTPWM", "SERVO_MIXER",
     NULL
 };
 

diff --git 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, that also enables servos, even if mixer doesn't
-    if (feature(FEATURE_SERVO_TILT))
+    // 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))
         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();
 }
 
@@ -380,6 +380,17 @@ 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]);
@@ -436,8 +447,8 @@ void writeServos(void)
             break;
 
         default:
-            // Two servos for SERVO_TILT, if enabled
-            if (feature(FEATURE_SERVO_TILT)) {
+            // otherwise, control the first two servos when SERVO_TILT or SERVO_MIXER is enabled
+            if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_SERVO_MIXER)) {
                 pwmWriteServo(0, servo[0]);
                 pwmWriteServo(1, servo[1]);
             }
@@ -463,6 +474,15 @@ 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];
@@ -498,9 +518,6 @@ 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
@@ -511,24 +528,35 @@ 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)
+            if (currentServoMixer[i].speed == 0) {
+                // directly use the input value if speed is not provided
                 currentOutput[i] = input[from];
-            else {
+            } else {
+                // apply speed constraints
                 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]);
             }
 
-            servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
-        } else
-            currentOutput[i] = 0;
-    }
+            // 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 rates
-    for (i = 0; i < MAX_SERVOS; i++) {
-        servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100;
-        servo[i] += servoMiddle(i);
+            // center the servo around its middle
+            servo[target] += servoMiddle(i);
+        }
     }
 }
 
@@ -554,29 +582,19 @@ void mixTable(void)
             motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
     }
 
-    // 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;
+    // 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);
     }
 
-    // do camstab
+    // set camstab servo output before applying servo mixer rules
     if (feature(FEATURE_SERVO_TILT)) {
-        // center at fixed position, or vary either pitch or roll by RC channel
-        servo[0] = servoMiddle(0);
-        servo[1] = servoMiddle(1);
-
+        // vary either pitch or roll by RC channel
         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;
@@ -588,20 +606,14 @@ 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)