From 2fef9d5fa97e957aa8731e42186deb2c9fbaca8e Mon Sep 17 00:00:00 2001
From: dongie <timecop@gmail.com>
Date: Wed, 11 Jun 2014 15:10:17 +0900
Subject: [PATCH] Fixing gimbal_flags not working when feature SERVO_TILT was
 enabled. This was done by properly calculating servo offset instead of
 depending on enabled features/mixers.

---
 src/drv_pwm.c | 11 ++++++++++-
 src/drv_pwm.h |  7 +++++++
 src/main.c    |  5 +++++
 src/mixer.c   | 11 ++++++-----
 src/mw.c      |  6 ++++--
 src/mw.h      |  1 +
 6 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/src/drv_pwm.c b/src/drv_pwm.c
index a0ebc4fe9..c2ee5c28b 100755
--- a/src/drv_pwm.c
+++ b/src/drv_pwm.c
@@ -1,3 +1,7 @@
+/*
+ * This file is part of baseflight
+ * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md
+ */
 #include "board.h"
 
 /*
@@ -361,7 +365,8 @@ bool pwmInit(drv_pwm_config_t *init)
         }
 
         if (init->extraServos && !init->airplane) {
-            // remap PWM5..8 as servos when used in extended servo mode
+            // remap PWM5..8 as servos when used in extended servo mode. 
+            // condition for airplane because airPPM already has these as servos
             if (port >= PWM5 && port <= PWM8)
                 mask = TYPE_S;
         }
@@ -390,6 +395,10 @@ bool pwmInit(drv_pwm_config_t *init)
     pwmWritePtr = pwmWriteStandard;
     if (init->motorPwmRate > 500)
         pwmWritePtr = pwmWriteBrushed;
+
+    // set return values in init struct
+    init->numServos = numServos;
+
     return false;
 }
 
diff --git a/src/drv_pwm.h b/src/drv_pwm.h
index 918d88f40..e7d7d9821 100755
--- a/src/drv_pwm.h
+++ b/src/drv_pwm.h
@@ -1,3 +1,7 @@
+/*
+ * This file is part of baseflight
+ * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md
+ */
 #pragma once
 
 #define MAX_MOTORS  12
@@ -22,6 +26,9 @@ typedef struct drv_pwm_config_t {
                          // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
     uint16_t servoCenterPulse;
     uint16_t failsafeThreshold;
+    
+    // OUT parameters, filled by driver
+    uint8_t numServos;
 } drv_pwm_config_t;
 
 // This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data.
diff --git a/src/main.c b/src/main.c
index 31e727497..fac6f8466 100755
--- a/src/main.c
+++ b/src/main.c
@@ -1,3 +1,7 @@
+/*
+ * This file is part of baseflight
+ * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md
+ */
 #include "board.h"
 #include "mw.h"
 
@@ -126,6 +130,7 @@ int main(void)
     }
 
     pwmInit(&pwm_params);
+    core.numServos = pwm_params.numServos;
 
     // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
     for (i = 0; i < RC_CHANS; i++)
diff --git a/src/mixer.c b/src/mixer.c
index cfbff6e07..a565ce8e5 100755
--- a/src/mixer.c
+++ b/src/mixer.c
@@ -1,3 +1,7 @@
+/*
+ * This file is part of baseflight
+ * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md
+ */
 #include "board.h"
 #include "mw.h"
 
@@ -468,13 +472,10 @@ void mixTable(void)
 
     // forward AUX1-4 to servo outputs (not constrained)
     if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
-        int offset = 0;
+        int offset = core.numServos - 4;
         // offset servos based off number already used in mixer types
         // airplane and servo_tilt together can't be used
-        if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
-            offset = 4;
-        else if (mixers[mcfg.mixerConfiguration].useServo)
-            offset = 2;
+        // calculate offset by taking 4 from core.numServos
         for (i = 0; i < 4; i++)
             pwmWriteServo(i + offset, rcData[AUX1 + i]);
     }
diff --git a/src/mw.c b/src/mw.c
index 4c7323598..4ca0a0db8 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -1,3 +1,7 @@
+/*
+ * This file is part of baseflight
+ * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md
+ */
 #include "board.h"
 #include "mw.h"
 
@@ -97,8 +101,6 @@ void annexCode(void)
     static int64_t mAhdrawnRaw = 0;
     static int32_t vbatCycleTime = 0;
 
-    int i;
-
     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
     if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
         prop2 = 100;
diff --git a/src/mw.h b/src/mw.h
index 54a29e4c2..7d27750cf 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -311,6 +311,7 @@ typedef struct core_t {
     uint8_t mpu6050_scale;                  // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver.
     uint8_t numRCChannels;                  // number of rc channels as reported by current input driver
     bool useServo;                          // feature SERVO_TILT or wing/airplane mixers will enable this
+    uint8_t numServos;                      // how many total hardware servos we have. used by mixer
 } core_t;
 
 typedef struct flags_t {