Merge remote-tracking branch 'multiwii/master'

Makefile

@@ -68,8 +68,7 @@ COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
 		   $(STDPERIPH_SRC)
 
 # Source files for the NAZE target
-NAZE_SRC	 = drv_spi.c \
+NAZE_SRC	 = drv_adc.c \
-		   drv_adc.c \
 		   drv_adxl345.c \
 		   drv_bma280.c \
 		   drv_bmp085.c \
@@ -82,6 +81,7 @@ NAZE_SRC	 = drv_spi.c \
 		   drv_mpu6050.c \
 		   drv_l3g4200d.c \
 		   drv_pwm.c \
+		   drv_spi.c \
 		   drv_timer.c \
 		   $(COMMON_SRC)
 
@@ -134,7 +134,7 @@ INCLUDE_DIRS	 = $(SRC_DIR) \
 		   $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x \
 
 ARCH_FLAGS	 = -mthumb -mcpu=cortex-m3
-BASE_CFLAGS		 = $(ARCH_FLAGS) \
+BASE_CFLAGS	 = $(ARCH_FLAGS) \
 		   $(addprefix -D,$(OPTIONS)) \
 		   $(addprefix -I,$(INCLUDE_DIRS)) \
 		   -Wall \

src/cli.c

@@ -124,6 +124,7 @@ const clivalue_t valueTable[] = {
     { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 },
     { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
     { "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 },
+    { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 },
     { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
     { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 },
     { "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 },
@@ -131,6 +132,7 @@ const clivalue_t valueTable[] = {
     { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, -1, 4 },
     { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 },
     { "telemetry_softserial", VAR_UINT8, &mcfg.telemetry_softserial, 0, 1 },
+    { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 },
     { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
     { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
     { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },

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 = 55;
+static const uint8_t EEPROM_CONF_VERSION = 56;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -193,10 +193,13 @@ static void resetConf(void)
     mcfg.power_adc_channel = 0;
     mcfg.serialrx_type = 0;
     mcfg.telemetry_softserial = 0;
+    mcfg.telemetry_switch = 0;
     mcfg.midrc = 1500;
     mcfg.mincheck = 1100;
     mcfg.maxcheck = 1900;
     mcfg.retarded_arm = 0;       // disable arm/disarm on roll left/right
+    mcfg.flaps_speed = 0;
+    mcfg.fixedwing_althold_dir = 1;
     // Motor/ESC/Servo
     mcfg.minthrottle = 1150;
     mcfg.maxthrottle = 1850;

src/gps.c

@@ -9,6 +9,7 @@
 #define GPS_TIMEOUT (2500)
 // How many entries in gpsInitData array below
 #define GPS_INIT_ENTRIES (5)
+#define GPS_BAUD_DELAY (100)
 
 typedef struct gpsInitData_t {
     uint32_t baudrate;
@@ -70,6 +71,7 @@ typedef struct gpsData_t {
     uint32_t lastLastMessage;       // last-last valid GPS message. Used to calculate delta.
     
     uint32_t state_position;           // incremental variable for loops
+    uint32_t state_ts;                    // timestamp for last state_position increment
     
 } gpsData_t;
 
@@ -83,6 +85,7 @@ static void gpsSetState(uint8_t state)
 {
     gpsData.state = state;
     gpsData.state_position = 0;
+    gpsData.state_ts = millis();
 }
 
 void gpsInit(uint8_t baudrate)
@@ -114,31 +117,34 @@ void gpsInitHardware(void)
             // nothing to do, just set baud rate and try receiving some stuff and see if it parses
             serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
             gpsSetState(GPS_RECEIVINGDATA);
-            return;
+            break;
 
         case GPS_UBLOX:
-            // UBX will run at mcfg.baudrate, it shouldn't be "autodetected". So here we force it to that rate
+            // UBX will run at mcfg.gps_baudrate, it shouldn't be "autodetected". So here we force it to that rate
 
-                // Wait until GPS transmit buffer is empty
+            // Wait until GPS transmit buffer is empty
-                if (!isSerialTransmitBufferEmpty(core.gpsport))
+            if (!isSerialTransmitBufferEmpty(core.gpsport))
-                    break;
+                break;
 
             if (gpsData.state == GPS_INITIALIZING) {
+                uint32_t m = millis();
+                if (m - gpsData.state_ts < GPS_BAUD_DELAY)
+                    return;
+
                 if (gpsData.state_position < GPS_INIT_ENTRIES) {
                     // try different speed to INIT
                     serialSetBaudRate(core.gpsport, gpsInitData[gpsData.state_position].baudrate);
                     // but print our FIXED init string for the baudrate we want to be at
-                    serialPrint(core.gpsport, gpsInitData[mcfg.gps_baudrate].ubx);
+                    serialPrint(core.gpsport, gpsInitData[gpsData.baudrateIndex].ubx);
 
                     gpsData.state_position++;
+                    gpsData.state_ts = m;
                 } else {
                     // we're now (hopefully) at the correct rate, next state will switch to it
-                    gpsData.baudrateIndex = mcfg.gps_baudrate;
                     gpsSetState(GPS_INITDONE);
                 }
             } else {
                 // GPS_INITDONE, set our real baud rate and push some ublox config strings
-
                 if (gpsData.state_position == 0)
                     serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
 

src/imu.c

@@ -315,7 +315,7 @@ static void getEstimatedAttitude(void)
     acc_calc(deltaT); // rotate acc vector into earth frame
 
     if (cfg.throttle_angle_correction) {
-        int cosZ = EstG.V.Z / (acc_1G * 100.0f);
+        int cosZ = ((int32_t)(EstG.V.Z * 100.0f)) / acc_1G;
         throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
     }
 }

src/mixer.c

@@ -43,12 +43,12 @@ static const motorMixer_t mixerY6[] = {
 };
 
 static const motorMixer_t mixerHex6P[] = {
-    { 1.0f, -1.0f,  0.866025f,  1.0f },     // REAR_R
+    { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
-    { 1.0f, -1.0f, -0.866025f, -1.0f },     // FRONT_R
+    { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
-    { 1.0f,  1.0f,  0.866025f,  1.0f },     // REAR_L
+    { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
-    { 1.0f,  1.0f, -0.866025f, -1.0f },     // FRONT_L
+    { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
-    { 1.0f,  0.0f, -0.866025f,  1.0f },     // FRONT
+    { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
-    { 1.0f,  0.0f,  0.866025f, -1.0f },     // REAR
+    { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 };
 
 static const motorMixer_t mixerY4[] = {
@@ -59,12 +59,12 @@ static const motorMixer_t mixerY4[] = {
 };
 
 static const motorMixer_t mixerHex6X[] = {
-    { 1.0f, -0.866025f,  1.0f,  1.0f },     // REAR_R
+    { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
-    { 1.0f, -0.866025f, -1.0f,  1.0f },     // FRONT_R
+    { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
-    { 1.0f,  0.866025f,  1.0f, -1.0f },     // REAR_L
+    { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
-    { 1.0f,  0.866025f, -1.0f, -1.0f },     // FRONT_L
+    { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
-    { 1.0f, -0.866025f,  0.0f, -1.0f },     // RIGHT
+    { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
-    { 1.0f,  0.866025f,  0.0f,  1.0f },     // LEFT
+    { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 };
 
 static const motorMixer_t mixerOctoX8[] = {
@@ -211,6 +211,14 @@ void mixerInit(void)
             }
         }
     }
+
+    // set flag that we're on something with wings
+    if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING ||
+        mcfg.mixerConfiguration == MULTITYPE_AIRPLANE)
+        f.FIXED_WING = 1;
+    else
+        f.FIXED_WING = 0;
+
     mixerResetMotors();
 }
 
@@ -463,7 +471,11 @@ void mixTable(void)
     // forward AUX1-4 to servo outputs (not constrained)
     if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
         int offset = 0;
-        if (feature(FEATURE_SERVO_TILT))
+        // 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;
         for (i = 0; i < 4; i++)
             pwmWriteServo(i + offset, rcData[AUX1 + i]);

src/mw.c

@@ -831,34 +831,42 @@ void loop(void)
             if (f.BARO_MODE) {
                 static uint8_t isAltHoldChanged = 0;
                 static int16_t AltHoldCorr = 0;
-                if (cfg.alt_hold_fast_change) {
+                if (!f.FIXED_WING) {
-                    // rapid alt changes
+                    // multirotor alt hold
-                    if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
+                    if (cfg.alt_hold_fast_change) {
-                        errorAltitudeI = 0;
+                        // rapid alt changes
-                        isAltHoldChanged = 1;
+                        if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
-                        rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral;
+                            errorAltitudeI = 0;
+                            isAltHoldChanged = 1;
+                            rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral;
+                        } else {
+                            if (isAltHoldChanged) {
+                                AltHold = EstAlt;
+                                isAltHoldChanged = 0;
+                            }
+                            rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
+                        }
                     } else {
-                        if (isAltHoldChanged) {
+                        // slow alt changes for apfags
+                        if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
+                            // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
+                            AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
+                            AltHold += AltHoldCorr / 2000;
+                            AltHoldCorr %= 2000;
+                            isAltHoldChanged = 1;
+                        } else if (isAltHoldChanged) {
                             AltHold = EstAlt;
+                            AltHoldCorr = 0;
                             isAltHoldChanged = 0;
                         }
                         rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
+                        rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
                     }
                 } else {
-                    // slow alt changes for apfags
+                    // handle fixedwing-related althold. UNTESTED! and probably wrong
-                    if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
+                    // most likely need to check changes on pitch channel and 'reset' althold similar to
-                        // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
+                    // how throttle does it on multirotor
-                        AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
+                    rcCommand[PITCH] += BaroPID * mcfg.fixedwing_althold_dir;
-                        AltHold += AltHoldCorr / 2000;
-                        AltHoldCorr %= 2000;
-                        isAltHoldChanged = 1;
-                    } else if (isAltHoldChanged) {
-                        AltHold = EstAlt;
-                        AltHoldCorr = 0;
-                        isAltHoldChanged = 0;
-                    }
-                    rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
-                    rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
                 }
             }
         }

src/mw.h

@@ -101,6 +101,7 @@ enum {
     BOXCALIB,
     BOXGOV,
     BOXOSD,
+    BOXTELEMETRY,
     CHECKBOXITEMS
 };
 
@@ -265,6 +266,7 @@ typedef struct master_t {
     uint16_t maxcheck;                      // maximum rc end
     uint8_t retarded_arm;                   // allow disarsm/arm on throttle down + roll left/right
     uint8_t flaps_speed;                    // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
+    int8_t fixedwing_althold_dir;           // +1 or -1 for pitch/althold gain. later check if need more than just sign
 
     uint8_t rssi_aux_channel;               // Read rssi from channel. 1+ = AUX1+, 0 to disable.
 
@@ -275,10 +277,10 @@ typedef struct master_t {
     uint32_t serial_baudrate;
     
     uint32_t softserial_baudrate;
-    uint8_t softserial_inverted;           // use inverted softserial input and output signals
+    uint8_t softserial_inverted;            // use inverted softserial input and output signals
-
-    uint8_t telemetry_softserial;               // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
 
+    uint8_t telemetry_softserial;           // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
+    uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
     config_t profile[3];                    // 3 separate profiles
     uint8_t current_profile;                // currently loaded profile
 
@@ -314,6 +316,7 @@ typedef struct flags_t {
     uint8_t SMALL_ANGLES_25;
     uint8_t CALIBRATE_MAG;
     uint8_t VARIO_MODE;
+    uint8_t FIXED_WING;                     // set when in flying_wing or airplane mode. currently used by althold selection code
 } flags_t;
 
 extern int16_t gyroZero[3];

src/serial.c

@@ -84,6 +84,7 @@ struct box_t {
     { BOXCALIB, "CALIB;", 17 },
     { BOXGOV, "GOVERNOR;", 18 },
     { BOXOSD, "OSD SW;", 19 },
+    { BOXTELEMETRY, "TELEMETRY;", 20 },
     { CHECKBOXITEMS, NULL, 0xFF }
 };
 
@@ -265,6 +266,8 @@ void serialInit(uint32_t baudrate)
     if (feature(FEATURE_INFLIGHT_ACC_CAL))
         availableBoxes[idx++] = BOXCALIB;
     availableBoxes[idx++] = BOXOSD;
+    if (feature(FEATURE_TELEMETRY && mcfg.telemetry_switch))
+        availableBoxes[idx++] = BOXTELEMETRY;
     numberBoxItems = idx;
 }
 
@@ -321,7 +324,8 @@ static void evaluateCommand(void)
     case MSP_SET_MISC:
         read16(); // powerfailmeter
         mcfg.minthrottle = read16();
-        read32(); // mcfg.maxthrottle, mcfg.mincommand
+        mcfg.maxthrottle = read16();
+        mcfg.mincommand = read16();
         cfg.failsafe_throttle = read16();
         read16();
         read32();
@@ -378,6 +382,7 @@ static void evaluateCommand(void)
                     rcOptions[BOXCALIB] << BOXCALIB |
                     rcOptions[BOXGOV] << BOXGOV |
                     rcOptions[BOXOSD] << BOXOSD |
+                    rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
                     f.ARMED << BOXARM;
         for (i = 0; i < numberBoxItems; i++) {
             int flag = (tmp & (1 << availableBoxes[i]));

src/telemetry.c

@@ -228,7 +228,11 @@ void initTelemetry(void)
 
 void updateTelemetryState(void)
 {
-    bool State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
+    bool State;
+    if (!mcfg.telemetry_switch)
+        State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
+    else
+        State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : rcOptions[BOXTELEMETRY];
 
     if (State != telemetryEnabled) {
         if (mcfg.telemetry_softserial == TELEMETRY_UART) {
@@ -246,7 +250,7 @@ static uint8_t cycleNum = 0;
 
 void sendTelemetry(void)
 {
-    if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED)
+    if (mcfg.telemetry_softserial == TELEMETRY_UART && ((!f.ARMED && !mcfg.telemetry_switch) || (mcfg.telemetry_switch && !rcOptions[BOXTELEMETRY])))
         return;
 
     if (serialTotalBytesWaiting(core.telemport) != 0)