initial take at configurable FEATURE_3D

completely untested and may attempt to kill you when enabled. no binary.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@360 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/board.h

@@ -70,6 +70,7 @@ typedef enum {
     FEATURE_TELEMETRY = 1 << 11,
     FEATURE_POWERMETER = 1 << 12,
     FEATURE_VARIO = 1 << 13,
+    FEATURE_3D = 1 << 14,
 } AvailableFeatures;
 
 typedef enum {

src/cli.c

@@ -44,7 +44,7 @@ const char * const mixerNames[] = {
 const char * const featureNames[] = {
     "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
     "SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
-    "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO",
+    "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
     NULL
 };
 
@@ -53,7 +53,6 @@ const char * const sensorNames[] = {
     "ACC", "BARO", "MAG", "SONAR", "GPS", NULL
 };
 
-// 
 const char * const accNames[] = {
     "", "ADXL345", "MPU6050", "MMA845x", NULL
 };
@@ -108,9 +107,13 @@ const clivalue_t valueTable[] = {
     { "mincommand", VAR_UINT16, &mcfg.mincommand, 0, 2000 },
     { "mincheck", VAR_UINT16, &mcfg.mincheck, 0, 2000 },
     { "maxcheck", VAR_UINT16, &mcfg.maxcheck, 0, 2000 },
-    { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
+    { "deadband3d_low", VAR_UINT16, &mcfg.deadband3d_low, 0, 2000 },
+    { "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, 0, 2000 },
+    { "neutral3d", VAR_UINT16, &mcfg.neutral3d, 0, 2000 },
+    { "deadband3d_throttle", VAR_UINT16, &mcfg.deadband3d_throttle, 0, 2000 },
     { "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 },
     { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 },
+    { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
     { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
     { "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 },
     { "spektrum_hires", VAR_UINT8, &mcfg.spektrum_hires, 0, 1 },

src/config.c

@@ -194,6 +194,10 @@ static void resetConf(void)
     mcfg.minthrottle = 1150;
     mcfg.maxthrottle = 1850;
     mcfg.mincommand = 1000;
+    mcfg.deadband3d_low = 1406;
+    mcfg.deadband3d_high = 1514;
+    mcfg.neutral3d = 1460;
+    mcfg.deadband3d_throttle = 50;
     mcfg.motor_pwm_rate = 400;
     mcfg.servo_pwm_rate = 50;
     // gps/nav stuff

src/mixer.c

@@ -166,6 +166,17 @@ void mixerInit(void)
                 currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i];
         }
     }
+
+    // in 3D mode, mixer gain has to be halved
+    if (feature(FEATURE_3D)) {
+        if (numberMotor > 1) {
+            for (i = 0; i < numberMotor; i++) {
+                currentMixer[i].pitch *= 0.5f;
+                currentMixer[i].roll *= 0.5f;
+                currentMixer[i].yaw *= 0.5f;
+            }
+        }
+    }
 }
 
 void mixerLoadMix(int index)
@@ -382,14 +393,22 @@ void mixTable(void)
     for (i = 0; i < numberMotor; i++) {
         if (maxMotor > mcfg.maxthrottle)     // this is a way to still have good gyro corrections if at least one motor reaches its max.
             motor[i] -= maxMotor - mcfg.maxthrottle;
-        motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
+        if (feature(FEATURE_3D)) {
-        if ((rcData[THROTTLE]) < mcfg.mincheck) {
+            if ((rcData[THROTTLE]) > 1500) {
-            if (!feature(FEATURE_MOTOR_STOP))
+                motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle);
-                motor[i] = mcfg.minthrottle;
+            } else {
-            else
+                motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low);
-                motor[i] = mcfg.mincommand;
+            }
+        } else {
+            motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
+            if ((rcData[THROTTLE]) < mcfg.mincheck) {
+                if (!feature(FEATURE_MOTOR_STOP))
+                    motor[i] = mcfg.minthrottle;
+                else
+                    motor[i] = mcfg.mincommand;
+            }
         }
         if (!f.ARMED)
-            motor[i] = mcfg.mincommand;
+            motor[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand;
     }
 }

src/mw.c

@@ -433,6 +433,7 @@ void loop(void)
     static uint32_t loopTime;
     uint16_t auxState = 0;
     static uint8_t GPSNavReset = 1;
+    bool isThrottleLow = false;
 
     // this will return false if spektrum is disabled. shrug.
     if (spektrumFrameComplete())
@@ -444,6 +445,12 @@ void loop(void)
         if (!feature(FEATURE_SPEKTRUM))
             computeRC();
 
+        // in 3D mode, we need to be able to disarm by switch at any time
+        if (feature(FEATURE_3D)) {
+            if (!rcOptions[BOXARM])
+                mwDisarm();
+        }
+
         // Failsafe routine
         if (feature(FEATURE_FAILSAFE)) {
             if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
@@ -481,7 +488,11 @@ void loop(void)
         rcSticks = stTmp;
 
         // perform actions
-        if (rcData[THROTTLE] < mcfg.mincheck) {
+        if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.midrc + mcfg.deadband3d_throttle)))
+            isThrottleLow = true;
+        else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.mincheck))
+            isThrottleLow = true;
+        if (isThrottleLow) {
             errorGyroI[ROLL] = 0;
             errorGyroI[PITCH] = 0;
             errorGyroI[YAW] = 0;

src/mw.h

@@ -238,6 +238,10 @@ typedef struct master_t {
     uint16_t minthrottle;                   // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
     uint16_t maxthrottle;                   // This is the maximum value for the ESCs at full power this value can be increased up to 2000
     uint16_t mincommand;                    // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
+    uint16_t deadband3d_low;                // min 3d value
+    uint16_t deadband3d_high;               // max 3d value
+    uint16_t neutral3d;                     // center 3d value
+    uint16_t deadband3d_throttle;           // default throttle deadband from MIDRC
     uint16_t motor_pwm_rate;                // The update rate of motor outputs (50-498Hz)
     uint16_t servo_pwm_rate;                // The update rate of servo outputs (50-498Hz)