merged in mwii2.3 generic servo handler. completely untested.

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

src/cli.c

@@ -150,28 +150,7 @@ const clivalue_t valueTable[] = {
     { "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
     { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
     { "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },
-    { "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
-    { "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
-    { "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
-    { "wing_left_min", VAR_UINT16, &cfg.wing_left_min, 0, 2000 },
-    { "wing_left_mid", VAR_UINT16, &cfg.wing_left_mid, 0, 2000 },
-    { "wing_left_max", VAR_UINT16, &cfg.wing_left_max, 0, 2000 },
-    { "wing_right_min", VAR_UINT16, &cfg.wing_right_min, 0, 2000 },
-    { "wing_right_mid", VAR_UINT16, &cfg.wing_right_mid, 0, 2000 },
-    { "wing_right_max", VAR_UINT16, &cfg.wing_right_max, 0, 2000 },
-    { "pitch_direction_l", VAR_INT8, &cfg.pitch_direction_l, -1, 1 },
-    { "pitch_direction_r", VAR_INT8, &cfg.pitch_direction_r, -1, 1 },
-    { "roll_direction_l", VAR_INT8, &cfg.roll_direction_l, -1, 1 },
-    { "roll_direction_r", VAR_INT8, &cfg.roll_direction_r, -1, 1 },
     { "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255},
-    { "gimbal_pitch_gain", VAR_INT8, &cfg.gimbal_pitch_gain, -100, 100 },
-    { "gimbal_roll_gain", VAR_INT8, &cfg.gimbal_roll_gain, -100, 100 },
-    { "gimbal_pitch_min", VAR_UINT16, &cfg.gimbal_pitch_min, 100, 3000 },
-    { "gimbal_pitch_max", VAR_UINT16, &cfg.gimbal_pitch_max, 100, 3000 },
-    { "gimbal_pitch_mid", VAR_UINT16, &cfg.gimbal_pitch_mid, 100, 3000 },
-    { "gimbal_roll_min", VAR_UINT16, &cfg.gimbal_roll_min, 100, 3000 },
-    { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
-    { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
     { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
     { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 },
     { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 },

src/config.c

@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 51;
+static const uint8_t EEPROM_CONF_VERSION = 52;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -83,7 +83,6 @@ void readEEPROM(void)
         lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000;     // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
     }
 
-    cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max);       //REAR
     setPIDController(cfg.pidController);
     GPS_set_pids();
 }
@@ -286,32 +285,9 @@ static void resetConf(void)
 
     cfg.yaw_direction = 1;
     cfg.tri_unarmed_servo = 1;
-    cfg.tri_yaw_middle = 1500;
-    cfg.tri_yaw_min = 1020;
-    cfg.tri_yaw_max = 2000;
-
-    // flying wing
-    cfg.wing_left_min = 1020;
-    cfg.wing_left_mid = 1500;
-    cfg.wing_left_max = 2000;
-    cfg.wing_right_min = 1020;
-    cfg.wing_right_mid = 1500;
-    cfg.wing_right_max = 2000;
-    cfg.pitch_direction_l = 1;
-    cfg.pitch_direction_r = -1;
-    cfg.roll_direction_l = 1;
-    cfg.roll_direction_r = 1;
 
     // gimbal
-    cfg.gimbal_pitch_gain = 10;
-    cfg.gimbal_roll_gain = 10;
     cfg.gimbal_flags = GIMBAL_NORMAL;
-    cfg.gimbal_pitch_min = 1020;
-    cfg.gimbal_pitch_max = 2000;
-    cfg.gimbal_pitch_mid = 1500;
-    cfg.gimbal_roll_min = 1020;
-    cfg.gimbal_roll_max = 2000;
-    cfg.gimbal_roll_mid = 1500;
 
     // gps/nav stuff
     cfg.gps_wp_radius = 200;

src/main.c

@@ -81,6 +81,8 @@ int main(void)
     pwmInit(&pwm_params);
 
     // 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++)
+        rcData[i] = 1502;
     rcReadRawFunc = pwmReadRawRC;
     core.numRCChannels = MAX_INPUTS;
 

src/mixer.c

@@ -3,7 +3,7 @@
 
 static uint8_t numberMotor = 0;
 int16_t motor[MAX_MOTORS];
-int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
+int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
 
 static motorMixer_t currentMixer[MAX_MOTORS];
 
@@ -130,6 +130,30 @@ const mixer_t mixers[] = {
     { 0, 0, NULL },                // MULTITYPE_CUSTOM
 };
 
+int16_t servoMiddle(int nr)
+{
+    // Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than
+    // the number of RC channels, it means the center value is taken FROM that RC channel (by its index)
+    if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS)
+        return rcData[cfg.servoConf[nr].middle];
+    else
+        return cfg.servoConf[nr].middle;
+}
+
+int servoDirection(int nr, int lr)
+{
+    // servo.rate is overloaded for servos that don't have a rate, but only need direction
+    // bit set = negative, clear = positive
+    // rate[2] = ???_direction
+    // rate[1] = roll_direction
+    // rate[0] = pitch_direction
+    // servo.rate is also used as gimbal gain multiplier (yeah)
+    if (cfg.servoConf[nr].rate & lr)
+        return -1;
+    else
+        return 1;
+}
+
 void mixerInit(void)
 {
     int i;
@@ -319,17 +343,17 @@ void mixTable(void)
     // airplane / servo mixes
     switch (mcfg.mixerConfiguration) {
         case MULTITYPE_BI:
-            servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000);   //LEFT
+            servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4);     // LEFT
-            servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000);   //RIGHT
+            servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5);     // RIGHT
             break;
 
         case MULTITYPE_TRI:
-            servo[5] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
+            servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR
             break;
 
         case MULTITYPE_GIMBAL:
-            servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
+            servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
-            servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
+            servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
             break;
 
         case MULTITYPE_AIRPLANE:
@@ -337,47 +361,47 @@ void mixTable(void)
             break;
 
         case MULTITYPE_FLYING_WING:
-            motor[0] = rcCommand[THROTTLE];
+            if (!f.ARMED)
+                servo[7] = mcfg.mincommand;
+            else
+                servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
+            motor[0] = servo[7];
             if (f.PASSTHRU_MODE) {
                 // do not use sensors for correction, simple 2 channel mixing
-                servo[0]  = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc);
+                servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]);
-                servo[1]  = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc);
+                servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]);
             } else {
                 // use sensors to correct (gyro only or gyro + acc)
-                servo[0]  = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL];
+                servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]);
-                servo[1]  = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL];
+                servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]);
             }
-            servo[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max);
+            servo[3] += servoMiddle(3);
-            servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max);
+            servo[4] += servoMiddle(4);
             break;
     }
 
     // do camstab
     if (feature(FEATURE_SERVO_TILT)) {
-        uint16_t aux[2] = { 0, 0 };
+        // center at fixed position, or vary either pitch or roll by RC channel
-
+        servo[0] = servoMiddle(0);
-        if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY))
+        servo[1] = servoMiddle(1);
-            aux[0] = rcData[AUX3] - mcfg.midrc;
-        if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
-            aux[1] = rcData[AUX4] - mcfg.midrc;
-
-        servo[0] = cfg.gimbal_pitch_mid + aux[0];
-        servo[1] = cfg.gimbal_roll_mid + aux[1];
 
         if (rcOptions[BOXCAMSTAB]) {
             if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
-                servo[0] -= (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16;
+                servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
-                servo[1] += (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 + cfg.gimbal_roll_gain * angle[ROLL] / 16;
+                servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
             } else {
-                servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
+                servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
-                servo[1] += cfg.gimbal_roll_gain * angle[ROLL]  / 16;
+                servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL]  / 50;
             }
         }
-
-        servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
-        servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
     }
 
+    // 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 = 0;
         if (feature(FEATURE_SERVO_TILT))

src/mw.c

@@ -16,7 +16,7 @@ int16_t telemTemperature1;      // gyro sensor temperature
 
 int16_t failsafeCnt = 0;
 int16_t failsafeEvents = 0;
-int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
+int16_t rcData[RC_CHANS];       // interval [1000;2000]
 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 int16_t lookupPitchRollRC[6];   // lookup table for expo & RC rate PITCH+ROLL
 int16_t lookupThrottleRC[11];   // lookup table for expo & mid THROTTLE
@@ -802,9 +802,6 @@ void loop(void)
         currentTime = micros();
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
-#ifdef MPU6050_DMP
-        mpu6050DmpLoop();
-#endif
 
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {

src/mw.h

@@ -4,11 +4,13 @@
 #define VBATFREQ 6        // to read battery voltage - nth number of loop iterations
 #define BARO_TAB_SIZE_MAX   48
 
-#define  VERSION  220
+#define  VERSION  230
 
 #define LAT  0
 #define LON  1
 
+#define RC_CHANS    (18)
+
 // Serial GPS only variables
 // navigation mode
 typedef enum NavigationMode
@@ -44,10 +46,8 @@ typedef enum MultiType
 
 typedef enum GimbalFlags {
     GIMBAL_NORMAL = 1 << 0,
-    GIMBAL_TILTONLY = 1 << 1,
+    GIMBAL_MIXTILT = 1 << 1,
-    GIMBAL_DISABLEAUX34 = 1 << 2,
+    GIMBAL_FORWARDAUX = 1 << 2,
-    GIMBAL_FORWARDAUX = 1 << 3,
-    GIMBAL_MIXTILT = 1 << 4,
 } GimbalFlags;
 
 /*********** RC alias *****************/
@@ -194,33 +194,9 @@ typedef struct config_t {
     // mixer-related configuration
     int8_t yaw_direction;
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
-    uint16_t tri_yaw_middle;                // tail servo center pos. - use this for initial trim
-    uint16_t tri_yaw_min;                   // tail servo min
-    uint16_t tri_yaw_max;                   // tail servo max
-
-    // flying wing related configuration
-    uint16_t wing_left_min;                 // min/mid/max servo travel
-    uint16_t wing_left_mid;
-    uint16_t wing_left_max;
-    uint16_t wing_right_min;
-    uint16_t wing_right_mid;
-    uint16_t wing_right_max;
-
-    int8_t pitch_direction_l;               // left servo - pitch orientation
-    int8_t pitch_direction_r;               // right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored)
-    int8_t roll_direction_l;                // left servo - roll orientation
-    int8_t roll_direction_r;                // right servo - roll orientation  (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
 
     // gimbal-related configuration
-    int8_t gimbal_pitch_gain;               // gimbal pitch servo gain (tied to angle) can be negative to invert movement
-    int8_t gimbal_roll_gain;                // gimbal roll servo gain (tied to angle) can be negative to invert movement
     uint8_t gimbal_flags;                   // in servotilt mode, various things that affect stuff
-    uint16_t gimbal_pitch_min;              // gimbal pitch servo min travel
-    uint16_t gimbal_pitch_max;              // gimbal pitch servo max travel
-    uint16_t gimbal_pitch_mid;              // gimbal pitch servo neutral value
-    uint16_t gimbal_roll_min;               // gimbal roll servo min travel
-    uint16_t gimbal_roll_max;               // gimbal roll servo max travel
-    uint16_t gimbal_roll_mid;               // gimbal roll servo neutral value
 
     // gps-related stuff
     uint16_t gps_wp_radius;                 // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
@@ -305,6 +281,7 @@ typedef struct core_t {
     serialPort_t *telemport;
     serialPort_t *rcvrport;
     bool useServo;
+    uint8_t numRCChannels;
 
 } core_t;
 
@@ -362,8 +339,8 @@ extern int16_t throttleAngleCorrection;
 extern int16_t headFreeModeHold;
 extern int16_t heading, magHold;
 extern int16_t motor[MAX_MOTORS];
-extern int16_t servo[8];
+extern int16_t servo[MAX_SERVOS];
-extern int16_t rcData[8];
+extern int16_t rcData[RC_CHANS];
 extern uint16_t rssi;                  // range: [0;1023]
 extern uint8_t vbat;
 extern int16_t telemTemperature1;      // gyro sensor temperature

src/serial.c

@@ -149,7 +149,6 @@ static const char pidnames[] =
 
 static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
 static uint8_t cmdMSP;
-static bool guiConnected = false;
 // signal that we're in cli mode
 uint8_t cliMode = 0;
 
@@ -468,17 +467,28 @@ static void evaluateCommand(void)
             serialize16(magADC[i]);
         break;
     case MSP_SERVO:
-        headSerialReply(16);
+        s_struct((uint8_t *)&servo, 16);
-        for (i = 0; i < 8; i++)
-            serialize16(servo[i]);
         break;
     case MSP_SERVO_CONF:
-        s_struct((uint8_t *)&cfg.servoConf, 56);      // struct servoConf is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
+        headSerialReply(56);
+        for (i = 0; i < MAX_SERVOS; i++) {
+            serialize16(cfg.servoConf[i].min);
+            serialize16(cfg.servoConf[i].max);
+            serialize16(cfg.servoConf[i].middle);
+            serialize8(cfg.servoConf[i].rate);
+        }
+        break;
+    case MSP_SET_SERVO_CONF:
+        headSerialReply(0);
+        for (i = 0; i < MAX_SERVOS; i++) {
+            cfg.servoConf[i].min = read16();
+            cfg.servoConf[i].max = read16();
+            cfg.servoConf[i].middle = read16();
+            cfg.servoConf[i].rate = read8();
+        }
         break;
     case MSP_MOTOR:
-        headSerialReply(16);
+        s_struct((uint8_t *)motor, 16);
-        for (i = 0; i < 8; i++)
-            serialize16(motor[i]);
         break;
     case MSP_RC:
         headSerialReply(16);
@@ -718,7 +728,6 @@ void serialCom(void)
             indRX = 0;
             checksum ^= c;
             c_state = HEADER_SIZE;      // the command is to follow
-            guiConnected = true;
         } else if (c_state == HEADER_SIZE) {
             cmdMSP = c;
             checksum ^= c;