merged multiwii GPS code from 1097. still no support for UBX, or GPS auto-config, soon.

added interrupt pins from mag/mma/mpu for rev4 hardware. nothing done with them yet - candidates for EXTI use
added tx buffer to UART2 (gps) in preparation for auto-config


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

src/cli.c

@@ -129,6 +129,7 @@ const clivalue_t valueTable[] = {
     { "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 },
     { "mpu6050_scale", VAR_UINT8, &cfg.mpu6050_scale, 0, 1 },
     { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
+    { "gps_type", VAR_UINT8, &cfg.gps_type, 0, 3 },
     { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
     { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },
     { "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 },

src/config.c

@@ -13,7 +13,7 @@ config_t cfg;
 const char rcChannelLetters[] = "AERT1234";
 
 static uint32_t enabledSensors = 0;
-uint8_t checkNewConf = 26;
+uint8_t checkNewConf = 27;
 
 void parseRcChannels(const char *input)
 {
@@ -184,6 +184,7 @@ void checkFirstTime(bool reset)
     cfg.gimbal_roll_mid = 1500;
 
     // gps/nav stuff
+    cfg.gps_type = 0; // NMEA
     cfg.gps_baudrate = 9600;
     cfg.gps_wp_radius = 200;
     cfg.gps_lpf = 20;
@@ -192,7 +193,7 @@ void checkFirstTime(bool reset)
     cfg.nav_speed_min = 100;
     cfg.nav_speed_max = 300;
 
-    // serial(uart1) baudrate
+    // serial (USART1) baudrate
     cfg.serial_baudrate = 115200;
 
     writeParams(0);

src/drv_hmc5883l.c

@@ -1,6 +1,7 @@
 #include "board.h"
 
 // HMC5883L, default address 0x1E
+// PB12 connected to MAG_DRDY on rev4 hardware
 
 #define MAG_ADDRESS 0x1E
 #define MAG_DATA_REGISTER 0x03
@@ -43,6 +44,14 @@ bool hmc5883lDetect(void)
 
 void hmc5883lInit(void)
 {
+    GPIO_InitTypeDef GPIO_InitStructure;
+
+    // PB12 - MAG_DRDY output on rev4 hardware
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
     delay(100);
     i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
     delay(50);

src/drv_mma845x.c

@@ -1,6 +1,7 @@
 #include "board.h"
 
 // MMA8452QT, Standard address 0x1C
+// ACC_INT2 routed to PA5
 
 #define MMA8452_ADDRESS     0x1C
 
@@ -71,6 +72,14 @@ bool mma8452Detect(sensor_t *acc)
 
 static void mma8452Init(void)
 {
+    GPIO_InitTypeDef GPIO_InitStructure;
+
+    // PA5 - ACC_INT2 output on rev3/4 hardware
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_Init(GPIOA, &GPIO_InitStructure);
+
     i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
     i2cWrite(MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
     i2cWrite(MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);

src/drv_mpu6050.c

@@ -1,7 +1,9 @@
 #include "board.h"
 
 // MPU6050, Standard address 0x68
+// MPU_INT on PB13 on rev4 hardware
 #define MPU6050_ADDRESS         0x68
+
 // Experimental DMP support
 // #define MPU6050_DMP
 
@@ -210,6 +212,14 @@ static void mpu6050AccAlign(int16_t * accData)
 
 static void mpu6050GyroInit(void)
 {
+    GPIO_InitTypeDef GPIO_InitStructure;
+
+    // PB13 - MPU_INT output on rev4 hardware
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_Init(GPIOB, &GPIO_InitStructure);
+
 #ifndef MPU6050_DMP
     i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(5);

src/drv_uart.c

@@ -9,7 +9,6 @@
 // Receive buffer, circular DMA
 volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
 uint32_t rxDMAPos = 0;
-
 volatile uint8_t txBuffer[UART_BUFFER_SIZE];
 uint32_t txBufferTail = 0;
 uint32_t txBufferHead = 0;
@@ -44,10 +43,8 @@ void uartInit(uint32_t speed)
     DMA_InitTypeDef DMA_InitStructure;
     NVIC_InitTypeDef NVIC_InitStructure;
 
-    // UART
     // USART1_TX    PA9
     // USART1_RX    PA10
-
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
@@ -153,8 +150,14 @@ void uartPrint(char *str)
 
 /* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
 uartReceiveCallbackPtr uart2Callback = NULL;
+#define UART2_BUFFER_SIZE    64
 
-void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
+volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
+uint32_t tx2BufferTail = 0;
+uint32_t tx2BufferHead = 0;
+bool uart2RxOnly = false;
+
+void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
 {
     NVIC_InitTypeDef NVIC_InitStructure;
     GPIO_InitTypeDef GPIO_InitStructure;
@@ -162,33 +165,65 @@ void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
 
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
 
+    uart2RxOnly = rxOnly;
+
     NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
     NVIC_Init(&NVIC_InitStructure);
 
+    // USART2_TX    PA2
+    // USART2_RX    PA3
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+    if (!rxOnly)
+        GPIO_Init(GPIOA, &GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
-    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
     GPIO_Init(GPIOA, &GPIO_InitStructure);
 
     USART_InitStructure.USART_BaudRate = speed;
     USART_InitStructure.USART_WordLength = USART_WordLength_8b;
     USART_InitStructure.USART_StopBits = USART_StopBits_1;
     USART_InitStructure.USART_Parity = USART_Parity_No;
-    USART_InitStructure.USART_Mode = USART_Mode_Rx;
+    USART_InitStructure.USART_Mode = USART_Mode_Rx | (rxOnly ? 0 : USART_Mode_Tx);
     USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
     USART_Init(USART2, &USART_InitStructure);
 
     USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
+    if (!rxOnly)
+        USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
     USART_Cmd(USART2, ENABLE);
     uart2Callback = func;
 }
 
+void uart2Write(uint8_t ch)
+{
+    if (uart2RxOnly)
+        return;
+
+    tx2Buffer[tx2BufferHead] = ch;
+    tx2BufferHead = (tx2BufferHead + 1) % UART2_BUFFER_SIZE;
+
+    USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
+}
+
 void USART2_IRQHandler(void)
 {
-    if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
+    uint16_t SR = USART2->SR;
+
+    if (SR & USART_IT_RXNE) {
         if (uart2Callback)
             uart2Callback(USART_ReceiveData(USART2));
     }
+    if (SR & USART_FLAG_TXE) {
+        if (tx2BufferTail != tx2BufferHead) {
+            USART2->DR = tx2Buffer[tx2BufferTail];
+            tx2BufferTail = (tx2BufferTail + 1) % UART2_BUFFER_SIZE;
+        } else {
+            USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
+        }
+    }
 }

src/drv_uart.h

@@ -1,5 +1,6 @@
 #pragma once
 
+// USART1
 void uartInit(uint32_t speed);
 uint16_t uartAvailable(void);
 bool uartTransmitEmpty(void);
@@ -7,4 +8,7 @@ uint8_t uartRead(void);
 uint8_t uartReadPoll(void);
 void uartWrite(uint8_t ch);
 void uartPrint(char *str);
-void uart2Init(uint32_t speed, uartReceiveCallbackPtr func);
+
+// USART2 (
+void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly);
+void uart2Write(uint8_t ch);

src/gps.c

@@ -11,7 +11,7 @@ static void GPS_set_pids(void);
 void gpsInit(uint32_t baudrate)
 {
     GPS_set_pids();
-    uart2Init(baudrate, GPS_NewData);
+    uart2Init(baudrate, GPS_NewData, false);
 
     // catch some GPS frames. TODO check this
     delay(500);
@@ -21,7 +21,7 @@ void gpsInit(uint32_t baudrate)
 
 /*-----------------------------------------------------------
  *
- * Multiwii GPS code - revision: 851
+ * Multiwii GPS code - revision: 1097
  *
  *-----------------------------------------------------------*/
 #define POSHOLD_IMAX           20       // degrees
@@ -45,6 +45,7 @@ static void GPS_calc_poshold(void);
 static void GPS_calc_nav_rate(int max_speed);
 static void GPS_update_crosstrack(void);
 static bool GPS_newFrame(char c);
+static bool GPS_NMEA_newFrame(char c);
 static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
 int32_t wrap_18000(int32_t error);
 static int32_t wrap_36000(int32_t angle);
@@ -56,91 +57,62 @@ typedef struct {
     float Imax;
 } PID_PARAM;
 
-static PID_PARAM posholdPID;
+static PID_PARAM posholdPID_PARAM;
-static PID_PARAM poshold_ratePID;
+static PID_PARAM poshold_ratePID_PARAM;
-static PID_PARAM navPID;
+static PID_PARAM navPID_PARAM;
 
-// AC_PID.h & AC_PID.cpp
 typedef struct {
-    float _integrator;          ///< integrator value
+    float integrator;          // integrator value
-    int32_t _last_input;        ///< last input for derivative
+    int32_t last_input;        // last input for derivative
-    float _last_derivative;     ///< last derivative for low-pass filter
+    float last_derivative;     // last derivative for low-pass filter
-    float _output;
+    float output;
-    float _derivative;
+    float derivative;
-} AC_PID;
+} PID;
 
-static float AC_PID_get_integrator(AC_PID * ac_pid)
+static PID posholdPID[2];
-{
+static PID poshold_ratePID[2];
-    return ac_pid->_integrator;
+static PID navPID[2];
-}
 
-static void AC_PID_set_integrator(AC_PID * ac_pid, float i)
+static int32_t get_P(int32_t error, PID_PARAM *pid)
-{
-    ac_pid->_integrator = i;
-}
-
-/// Low pass filter cut frequency for derivative calculation.
-// static const float ac_pid_filter = 1.0f / (2.0f * M_PI * (float)cfg.gps_lpf); // Set to  "1 / ( 2 * PI * f_cut )"
-#define AC_PID_FILTER       (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
-
-/// Iterate the PID, return the new control value
-///
-/// Positive error produces positive output.
-///
-/// @param error        The measured error value
-/// @param dt           The time delta in milliseconds (note
-///                                     that update interval cannot be more
-///                                     than 65.535 seconds due to limited range
-///                                     of the data type).
-/// @param scaler       An arbitrary scale factor
-///
-/// @returns            The updated control output.
-///
-static int32_t AC_PID_get_p(AC_PID * ac_pid, int32_t error, PID_PARAM * pid)
 {
     return (float)error * pid->kP;
 }
 
-static int32_t AC_PID_get_i(AC_PID * ac_pid, int32_t error, float *dt, PID_PARAM * pid)
+static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param)
 {
-    ac_pid->_integrator += ((float) error * pid->kI) * *dt;
+    pid->integrator += ((float)error * pid_param->kI) * *dt;
-    if (ac_pid->_integrator < -pid->Imax) {
+    pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax);
-        ac_pid->_integrator = -pid->Imax;
+    return pid->integrator;
-    } else if (ac_pid->_integrator > pid->Imax) {
-        ac_pid->_integrator = pid->Imax;
-    }
-    return ac_pid->_integrator;
 }
 
-static int32_t AC_PID_get_d(AC_PID * ac_pid, int32_t input, float *dt, PID_PARAM * pid)
+static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
 {
-    ac_pid->_derivative = (input - ac_pid->_last_input) / *dt;
+    pid->derivative = (input - pid->last_input) / *dt;
+
+    // Low pass filter cut frequency for derivative calculation
+    // Set to  "1 / ( 2 * PI * gps_lpf )"
+#define PID_FILTER       (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
     // discrete low pass filter, cuts out the
     // high frequency noise that can drive the controller crazy
-    ac_pid->_derivative = ac_pid->_last_derivative + (*dt / (AC_PID_FILTER + *dt)) * (ac_pid->_derivative - ac_pid->_last_derivative);
+    pid->derivative = pid->last_derivative + (*dt / (PID_FILTER + *dt)) * (pid->derivative - pid->last_derivative);
     // update state
-    ac_pid->_last_input = input;
+    pid->last_input = input;
-    ac_pid->_last_derivative = ac_pid->_derivative;
+    pid->last_derivative = pid->derivative;
     // add in derivative component
-    return pid->kD * ac_pid->_derivative;
+    return pid_param->kD * pid->derivative;
 }
 
-/// Reset the PID integrator
+static void reset_PID(PID *pid)
-///
-static void AC_PID_reset(AC_PID * ac_pid)
 {
-    ac_pid->_integrator = 0;
+    pid->integrator = 0;
-    ac_pid->_last_input = 0;
+    pid->last_input = 0;
-    ac_pid->_last_derivative = 0;
+    pid->last_derivative = 0;
 }
 
 #define _X 1
 #define _Y 0
 
 /****************** PI and PID controllers for GPS ********************///32938 -> 33160
-static AC_PID pi_poshold[2];
-static AC_PID pid_poshold_rate[2];
-static AC_PID pid_nav[2];
 
 #define RADX100                    0.000174532925f
 #define CROSSTRACK_GAIN            1
@@ -216,16 +188,10 @@ void GPS_NewData(uint16_t c)
         else
             GPS_update = 1;
         if (f.GPS_FIX && GPS_numSat >= 5) {
-            if (!f.ARMED) {
+            if (!f.ARMED)
                 f.GPS_FIX_HOME = 0;
-            }
+            if (!f.GPS_FIX_HOME && f.ARMED)
-            if (!f.GPS_FIX_HOME && f.ARMED) {
+                GPS_reset_home_position();
-                f.GPS_FIX_HOME = 1;
-                GPS_home[LAT] = GPS_coord[LAT];
-                GPS_home[LON] = GPS_coord[LON];
-                GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
-                nav_takeoff_bearing = heading; // save takeoff heading
-            }
             // Apply moving average filter to GPS data
 #if defined(GPS_FILTERING)
             GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
@@ -259,6 +225,11 @@ void GPS_NewData(uint16_t c)
             GPS_distanceToHome = dist / 100;
             GPS_directionToHome = dir / 100;
             
+            if (!f.GPS_FIX_HOME) {      // If we don't have home set, do not display anything
+                GPS_distanceToHome = 0;
+                GPS_directionToHome = 0;
+            }
+
             // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
             GPS_calc_velocity();
 
@@ -306,6 +277,7 @@ void GPS_reset_home_position(void)
     if (f.GPS_FIX && GPS_numSat >= 5) {
         GPS_home[LAT] = GPS_coord[LAT];
         GPS_home[LON] = GPS_coord[LON];
+        GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
         nav_takeoff_bearing = heading;              // save takeoff heading
         // Set ground altitude
         f.GPS_FIX_HOME = 1;
@@ -319,30 +291,30 @@ void GPS_reset_nav(void)
 
     for (i = 0; i < 2; i++) {
         GPS_angle[i] = 0;
+        nav_rated[i] = 0;
         nav[i] = 0;
-        AC_PID_reset(&pi_poshold[i]);
+        reset_PID(&posholdPID[i]);
-        AC_PID_reset(&pid_poshold_rate[i]);
+        reset_PID(&poshold_ratePID[i]);
-        AC_PID_reset(&pid_nav[i]);
+        reset_PID(&navPID[i]);
     }
 }
 
 //Get the relevant P I D values and set the PID controllers
 static void GPS_set_pids(void)
 {
-    posholdPID.kP = (float) cfg.P8[PIDPOS] / 100.0f;
+    posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
-    posholdPID.kI = (float) cfg.I8[PIDPOS] / 100.0f;
+    posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;
-    posholdPID.Imax = POSHOLD_RATE_IMAX * 100;
+    posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 
-    poshold_ratePID.kP = (float) cfg.P8[PIDPOSR] / 10.0f;
+    poshold_ratePID_PARAM.kP = (float)cfg.P8[PIDPOSR] / 10.0f;
-    poshold_ratePID.kI = (float) cfg.I8[PIDPOSR] / 100.0f;
+    poshold_ratePID_PARAM.kI = (float)cfg.I8[PIDPOSR] / 100.0f;
-    poshold_ratePID.kD = (float) cfg.D8[PIDPOSR] / 1000.0f;
+    poshold_ratePID_PARAM.kD = (float)cfg.D8[PIDPOSR] / 1000.0f;
-    poshold_ratePID.Imax = POSHOLD_RATE_IMAX * 100;
+    poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
-
-    navPID.kP = (float) cfg.P8[PIDNAVR] / 10.0f;
-    navPID.kI = (float) cfg.I8[PIDNAVR] / 100.0f;
-    navPID.kD = (float) cfg.D8[PIDNAVR] / 1000.0f;
-    navPID.Imax = POSHOLD_RATE_IMAX * 100;
 
+    navPID_PARAM.kP = (float)cfg.P8[PIDNAVR] / 10.0f;
+    navPID_PARAM.kI = (float)cfg.I8[PIDNAVR] / 100.0f;
+    navPID_PARAM.kD = (float)cfg.D8[PIDNAVR] / 1000.0f;
+    navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 }
 
 // OK here is the onboard GPS code
@@ -359,7 +331,7 @@ static void GPS_set_pids(void)
 //
 static void GPS_calc_longitude_scaling(int32_t lat)
 {
-    float rads = (abs((float)lat)) * (0.0174532925f / 10000000.0f);
+    float rads = (abs((float)lat) / 10000000.0f) * 0.0174532925f;
     GPS_scaleLonDown = cosf(rads);
 }
 
@@ -464,28 +436,28 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
 //
 static void GPS_calc_poshold(void)
 {
-    int32_t p, i, d;
+    int32_t d;
-    int32_t output;
     int32_t target_speed;
     int axis;
 
     for (axis = 0; axis < 2; axis++) {
-        target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID);       // calculate desired speed from lon error
+        target_speed = get_P(error[axis], &posholdPID_PARAM);       // calculate desired speed from lon error
         rate_error[axis] = target_speed - actual_speed[axis];       // calc the speed error
 
-        p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
+        nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +
-        i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis] + error[axis], &dTnav, &poshold_ratePID);
+                    get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
-        d = AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
+        d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
         d = constrain(d, -2000, 2000);
+
         // get rid of noise
 #if defined(GPS_LOW_SPEED_D_FILTER)
         if (abs(actual_speed[axis]) < 50)
             d = 0;
 #endif
-        output = p + i + d;
 
-        nav[axis] = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX);
+        nav[axis] +=d;
-        AC_PID_set_integrator(&pid_nav[axis], AC_PID_get_integrator(&pid_poshold_rate[axis]));
+        nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
+        navPID[axis].integrator = poshold_ratePID[axis].integrator;
     }
 }
 
@@ -510,11 +482,12 @@ static void GPS_calc_nav_rate(int max_speed)
         rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
         rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
         // P + I + D
-        nav[axis] = AC_PID_get_p(&pid_nav[axis], rate_error[axis], &navPID)
+        nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +
-            + AC_PID_get_i(&pid_nav[axis], rate_error[axis], &dTnav, &navPID)
+                    get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +
-            + AC_PID_get_d(&pid_nav[axis], rate_error[axis], &dTnav, &navPID);
+                    get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
+
         nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
-        AC_PID_set_integrator(&pid_poshold_rate[axis], AC_PID_get_integrator(&pid_nav[axis]));
+        poshold_ratePID[axis].integrator = navPID[axis].integrator;
     }
 }
 
@@ -550,7 +523,6 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
     // max_speed is default 400 or 4m/s
     if (_slow) {
         max_speed = min(max_speed, wp_distance / 2);
-        max_speed = max(max_speed, 0);
     } else {
         max_speed = min(max_speed, wp_distance);
         max_speed = max(max_speed, cfg.nav_speed_min);      // go at least 100cm/s
@@ -694,6 +666,19 @@ static uint8_t hex_c(uint8_t n)
     return n;
 }
 
+static bool GPS_newFrame(char c)
+{
+    switch (cfg.gps_type) {
+        case 0: // NMEA
+            return GPS_NMEA_newFrame(c);
+        
+        case 1: // UBX
+            return false; // GPS_UBLOX_newFrame(c);
+    }
+
+    return false;
+}
+
 /* This is a light implementation of a GPS frame decoding
    This should work with most of modern GPS devices configured to output NMEA frames.
    It assumes there are some NMEA GGA frames to decode on the serial bus
@@ -709,7 +694,7 @@ static uint8_t hex_c(uint8_t n)
 #define FRAME_GGA  1
 #define FRAME_RMC  2
 
-static bool GPS_newFrame(char c)
+static bool GPS_NMEA_newFrame(char c)
 {
     uint8_t frameOK = 0;
     static uint8_t param = 0, offset = 0, parity = 0;
@@ -746,7 +731,9 @@ static bool GPS_newFrame(char c)
             }
         } else if (frame == FRAME_RMC) {
             if (param == 7) {
-                GPS_speed = ((uint32_t) grab_fields(string, 1) * 5144L) / 1000L;    // speed in cm/s added by Mis
+                GPS_speed = (grab_fields(string, 1) * 5144L) / 1000L;   // speed in cm/s added by Mis
+            } else if (param == 8) {
+                GPS_ground_course = grab_fields(string, 1);             // ground course deg * 10
             }
         }
         param++;

src/mw.c

@@ -17,7 +17,7 @@ int16_t telemTemperature1;      // gyro sensor temperature
 
 int16_t failsafeCnt = 0;
 int16_t failsafeEvents = 0;
-int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
+int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // 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
@@ -487,6 +487,10 @@ void loop(void)
         } else {
             f.PASSTHRU_MODE = 0;
         }
+        
+        if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
+            f.HEADFREE_MODE = 0;
+        }
     } else {                    // not in rc loop
         static int8_t taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
         switch (taskOrder++ % 4) {
@@ -531,7 +535,6 @@ void loop(void)
         currentTime = micros();
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
-    
 #ifdef MPU6050_DMP
         mpu6050DmpLoop();
 #endif
@@ -554,7 +557,7 @@ void loop(void)
 #ifdef BARO
         if (sensors(SENSOR_BARO)) {
             if (f.BARO_MODE) {
-                if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
+                if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
                     f.BARO_MODE = 0;   // so that a new althold reference is defined
                 }
                 rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
@@ -564,7 +567,7 @@ void loop(void)
 
         if (sensors(SENSOR_GPS)) {
             // Check that we really need to navigate ?
-            if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || (!f.GPS_FIX_HOME)) {
+            if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) {
                 // If not. Reset nav loops and all nav related parameters
                 GPS_reset_nav();
             } else {

src/mw.h

@@ -170,7 +170,8 @@ typedef struct config_t {
     uint16_t gimbal_roll_mid;               // gimbal roll servo neutral value
 
     // gps-related stuff
-    uint32_t gps_baudrate;
+    uint8_t gps_type;                       // Type of GPS hardware. 0: NMEA 1: UBX 2+ ??
+    uint32_t gps_baudrate;                  // GPS baudrate
     uint16_t gps_wp_radius;                 // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
     uint8_t gps_lpf;                        // Low pass filter cut frequency for derivative calculation (default 20Hz)
     uint8_t nav_slew_rate;                  // Adds a rate control to nav output, will smoothen out nav angle spikes

src/spektrum.c

@@ -27,7 +27,7 @@ void spektrumInit(void)
         spek_chan_mask = 0x03;
     }
 
-    uart2Init(115200, spektrumDataReceive);
+    uart2Init(115200, spektrumDataReceive, true);
 }
 
 // UART2 Receive ISR callback