Merge remote-tracking branch 'multiwii/master'

Conflicts:
	obj/baseflight.hex
	src/sensors.c

README.md

@@ -0,0 +1,6 @@
+baseflight
+==========
+
+32 bit fork of the MultiWii RC flight controller firmware
+
+Before making any contributions, take a note of the https://github.com/multiwii/baseflight/wiki/CodingStyle

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 = 61;
+static const uint8_t EEPROM_CONF_VERSION = 62;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -279,7 +279,8 @@ static void resetConf(void)
     cfg.yawdeadband = 0;
     cfg.alt_hold_throttle_neutral = 40;
     cfg.alt_hold_fast_change = 1;
-    cfg.throttle_angle_correction = 0;      // could be 40
+    cfg.throttle_correction_value = 0;      // could 10 with althold or 40 for fpv
+    cfg.throttle_correction_angle = 800;    // could be 80.0 deg with atlhold or 45.0 for fpv
 
     // Failsafe Variables
     cfg.failsafe_delay = 10;                // 1sec

src/flight_imu.c

@@ -21,6 +21,7 @@ int32_t vario = 0;                      // variometer in cm/s
 int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
 float magneticDeclination = 0.0f;       // calculated at startup from config
 float accVelScale;
+float throttleAngleScale;
 
 // **************
 // gyro+acc IMU
@@ -36,6 +37,7 @@ void imuInit(void)
 {
     accZ_25deg = acc_1G * cosf(RAD * 25.0f);
     accVelScale = 9.80665f / acc_1G / 10000.0f;
+    throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);
 
 #ifdef MAG
     // if mag sensor is enabled, use it
@@ -303,9 +305,19 @@ static void getEstimatedAttitude(void)
 
     acc_calc(deltaT); // rotate acc vector into earth frame
 
-    if (cfg.throttle_angle_correction) {
-        int cosZ = ((int32_t)(EstG.V.Z * 100.0f)) / acc_1G;
-        throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
+    if (cfg.throttle_correction_value) {
+
+        float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
+
+        if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
+            throttleAngleCorrection = 0;
+        } else {
+            int angle = lrintf(acosf(cosZ) * throttleAngleScale);
+            if (angle > 900)
+                angle = 900;
+            throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f)) ;
+        }
+
     }
 }
 

src/gps_common.c

@@ -91,7 +91,8 @@ void gpsInit(uint8_t baudrateIndex)
         mode = MODE_RX;
 
     gpsSetPIDs();
-    core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrateIndex].baudrate, mode);
+    // Open GPS UART, no callback - buffer will be read out in gpsThread()
+    core.gpsport = uartOpen(USART2, NULL, gpsInitData[baudrateIndex].baudrate, mode);
     // signal GPS "thread" to initialize when it gets to it
     gpsSetState(GPS_INITIALIZING);
 }
@@ -156,6 +157,12 @@ void gpsInitHardware(void)
 
 void gpsThread(void)
 {
+    // read out available GPS bytes
+    if (core.gpsport) {
+        while (serialTotalBytesWaiting(core.gpsport))
+            gpsNewData(serialRead(core.gpsport));
+    }
+
     switch (gpsData.state) {
         case GPS_UNKNOWN:
             break;
@@ -534,9 +541,6 @@ int8_t gpsSetPassthrough(void)
     if (gpsData.state != GPS_RECEIVINGDATA)
         return -1;
 
-    // get rid of callback
-    core.gpsport->callback = NULL;
-
     LED0_OFF;
     LED1_OFF;
 
@@ -844,8 +848,10 @@ uint32_t GPS_coord_to_degrees(char* s)
     int i;
 
     // scan for decimal point or end of field
-    for (p = s; isdigit((unsigned char)*p); p++)
-        ;
+    for (p = s; isdigit((unsigned char)*p); p++) {
+        if (p >= s + 15)
+            return 0; // stop potential fail
+    }
     q = s;
 
     // convert degrees
@@ -890,6 +896,8 @@ static uint32_t grab_fields(char *src, uint8_t mult)
         tmp *= 10;
         if (src[i] >= '0' && src[i] <= '9')
             tmp += src[i] - '0';
+        if (i >= 15) 
+            return 0; // out of bounds
     }
     return tmp;
 }

src/mw.c

@@ -768,8 +768,6 @@ void loop(void)
         }
     } else {                    // not in rc loop
         static int taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
-        if (taskOrder > 4)
-            taskOrder -= 5;
         switch (taskOrder) {
         case 0:
             taskOrder++;
@@ -799,7 +797,7 @@ void loop(void)
                 break;
             }
         case 4:
-            taskOrder++;
+            taskOrder = 0;
 #ifdef SONAR
             if (sensors(SENSOR_SONAR)) {
                 Sonar_update();
@@ -884,7 +882,7 @@ void loop(void)
         }
 #endif
 
-        if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
+        if (cfg.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
             rcCommand[THROTTLE] += throttleAngleCorrection;
         }
 

src/mw.h

@@ -186,7 +186,8 @@ typedef struct config_t {
     uint8_t yawdeadband;                    // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
     uint8_t alt_hold_throttle_neutral;      // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
-    uint8_t throttle_angle_correction;      //
+    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
+    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
 
     // Servo-related stuff
     servoParam_t servoConf[8];              // servo configuration

src/serial_cli.c

@@ -161,7 +161,8 @@ const clivalue_t valueTable[] = {
     { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
     { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
     { "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 },
-    { "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 },
+    { "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 },
+    { "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 },
     { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
     { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
     { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },