committed (untested) GPS support by sbaron;

fix for channel map cli stuff by simonk.
reindented some code, so changes are large.

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

baseflight.uvopt

@@ -73,7 +73,7 @@
       <OPTFL>
         <tvExp>1</tvExp>
         <tvExpOptDlg>0</tvExpOptDlg>
-        <IsCurrentTarget>1</IsCurrentTarget>
+        <IsCurrentTarget>0</IsCurrentTarget>
       </OPTFL>
       <CpuCode>255</CpuCode>
       <Books>
@@ -279,7 +279,7 @@
       <OPTFL>
         <tvExp>1</tvExp>
         <tvExpOptDlg>0</tvExpOptDlg>
-        <IsCurrentTarget>0</IsCurrentTarget>
+        <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>
       <CpuCode>255</CpuCode>
       <Books>
@@ -318,7 +318,7 @@
         <tRbox>1</tRbox>
         <sRunDeb>0</sRunDeb>
         <sLrtime>0</sLrtime>
-        <nTsel>7</nTsel>
+        <nTsel>1</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -329,7 +329,7 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>Segger\JL2CM3.dll</pMon>
+        <pMon>BIN\UL2CM3.DLL</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
@@ -355,7 +355,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>UL2CM3</Key>
-          <Name>-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</Name>
+          <Name>-UV0168AVR -O206 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO11 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint>
@@ -558,10 +558,10 @@
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
-      <ColumnNumber>0</ColumnNumber>
+      <ColumnNumber>12</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
       <TopLine>39</TopLine>
-      <CurrentLine>46</CurrentLine>
+      <CurrentLine>50</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\mw.c</PathWithFileName>
       <FilenameWithoutPath>mw.c</FilenameWithoutPath>
@@ -600,10 +600,10 @@
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
-      <ColumnNumber>14</ColumnNumber>
+      <ColumnNumber>18</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
       <TopLine>1</TopLine>
-      <CurrentLine>18</CurrentLine>
+      <CurrentLine>7</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\board.h</PathWithFileName>
       <FilenameWithoutPath>board.h</FilenameWithoutPath>
@@ -637,8 +637,8 @@
       <Focus>0</Focus>
       <ColumnNumber>22</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
-      <TopLine>1</TopLine>
+      <TopLine>0</TopLine>
-      <CurrentLine>1</CurrentLine>
+      <CurrentLine>0</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drv_adc.c</PathWithFileName>
       <FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
@@ -693,8 +693,8 @@
       <Focus>0</Focus>
       <ColumnNumber>0</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
-      <TopLine>137</TopLine>
+      <TopLine>0</TopLine>
-      <CurrentLine>143</CurrentLine>
+      <CurrentLine>0</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
       <FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
@@ -707,8 +707,8 @@
       <Focus>0</Focus>
       <ColumnNumber>0</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
-      <TopLine>27</TopLine>
+      <TopLine>0</TopLine>
-      <CurrentLine>45</CurrentLine>
+      <CurrentLine>0</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drv_mpu3050.c</PathWithFileName>
       <FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
@@ -735,8 +735,8 @@
       <Focus>0</Focus>
       <ColumnNumber>0</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
-      <TopLine>42</TopLine>
+      <TopLine>0</TopLine>
-      <CurrentLine>43</CurrentLine>
+      <CurrentLine>0</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drv_system.c</PathWithFileName>
       <FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
@@ -966,8 +966,8 @@
       <Focus>0</Focus>
       <ColumnNumber>0</ColumnNumber>
       <tvExpOptDlg>0</tvExpOptDlg>
-      <TopLine>116</TopLine>
+      <TopLine>0</TopLine>
-      <CurrentLine>133</CurrentLine>
+      <CurrentLine>0</CurrentLine>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\baseflight_startups\startup_stm32f10x_md.s</PathWithFileName>
       <FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>

baseflight.uvproj

@@ -754,7 +754,7 @@
             <RestoreToolbox>1</RestoreToolbox>
           </Target>
           <RunDebugAfterBuild>0</RunDebugAfterBuild>
-          <TargetSelection>7</TargetSelection>
+          <TargetSelection>1</TargetSelection>
           <SimDlls>
             <CpuDll></CpuDll>
             <CpuDllArguments></CpuDllArguments>
@@ -768,7 +768,7 @@
             <PeripheralDll></PeripheralDll>
             <PeripheralDllArguments></PeripheralDllArguments>
             <InitializationFile></InitializationFile>
-            <Driver>Segger\JL2CM3.dll</Driver>
+            <Driver>BIN\UL2CM3.DLL</Driver>
           </TargetDlls>
         </DebugOption>
         <Utilities>
@@ -778,9 +778,9 @@
             <RunIndependent>0</RunIndependent>
             <UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
             <Capability>1</Capability>
-            <DriverSelection>4099</DriverSelection>
+            <DriverSelection>4096</DriverSelection>
           </Flash1>
-          <Flash2>Segger\JL2CM3.dll</Flash2>
+          <Flash2>BIN\UL2CM3.DLL</Flash2>
           <Flash3>"" ()</Flash3>
           <Flash4></Flash4>
         </Utilities>

src/board.h

@@ -29,6 +29,7 @@ typedef enum {
     FEATURE_CAMTRIG = 1 << 6,
     FEATURE_GYRO_SMOOTHING = 1 << 7,
     FEATURE_LED_RING = 1 << 8,
+    FEATURE_GPS = 1 << 9,
 } AvailableFeatures;
 
 typedef void (* sensorInitFuncPtr)(void);

src/cli.c

@@ -35,7 +35,7 @@ const char *mixerNames[] = {
 // sync this with AvailableFeatures enum from board.h
 const char *featureNames[] = {
     "PPM", "VBAT", "INFLIGHT_ACC_CAL", "DIGITAL_SERVO", "MOTOR_STOP",
-    "SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING",
+    "SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
     NULL
 };
 
@@ -51,7 +51,7 @@ const clicmd_t cmdTable[] = {
     { "exit", "", cliExit },
     { "feature", "list or -val or val", cliFeature },
     { "help", "", cliHelp },
-    { "map", "mapping of first 4 channels", cliMap },
+    { "map", "mapping of rc channel order", cliMap },
     { "mixer", "mixer name or list", cliMixer },
     { "save", "save and reboot", cliSave },
     { "set", "name=value or blank for list", cliSet },
@@ -113,6 +113,8 @@ const clivalue_t valueTable[] = {
 static void cliSetVar(const clivalue_t *var, const int32_t value);
 static void cliPrintVar(const clivalue_t *var);
 
+#ifndef HAVE_ITOA_FUNCTION
+
 /*
 ** The following two functions together make up an itoa()
 ** implementation. Function i2a() is a 'private' function
@@ -147,6 +149,8 @@ char *itoa(int i, char *a, int r)
     return a;
 } 
 
+#endif
+
 static void cliPrompt(void)
 {
     uartPrint("\r\n# ");
@@ -258,32 +262,24 @@ static void cliMap(char *cmdline)
 
     len = strlen(cmdline);
 
-    if (len == 0 || len != 8) {
+    if (len == 8) {
+        // uppercase it
+        for (i = 0; i < 8; i++)
+            cmdline[i] = toupper(cmdline[i]);
+        for (i = 0; i < 8; i++) {
+            if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
+                continue;
+            uartPrint("Must be any order of AETR1234\r\n");
+            return;
+        }
+        parseRcChannels(cmdline);
+    }
     uartPrint("Current assignment: ");
     for (i = 0; i < 8; i++)
         out[cfg.rcmap[i]] = rcChannelLetters[i];
     out[i] = '\0';
     uartPrint(out);
     uartPrint("\r\n");
-        return;
-    } else {
-        bool fail = false;
-        // uppercase it
-        for (i = 0; i < 8; i++) {
-            cmdline[i] = toupper(cmdline[i]);
-            if (!strchr(rcChannelLetters, cmdline[i])) {
-                fail = true;
-                break;
-            }
-        }
-
-        if (fail)
-            uartPrint("Must be any order of AETR1234\r\n");
-        else {
-            parseRcChannels(cmdline);
-            cliMap("");
-        }
-    }
 }
 
 static void cliMixer(char *cmdline)

src/gps.c

@@ -0,0 +1,249 @@
+#include "board.h"
+#include "mw.h"
+
+#ifndef PI
+#define PI 3.14159265358979323846
+#endif
+
+#ifndef sq
+#define sq(x) ((x)*(x))
+#endif
+
+static void GPS_NewData(uint16_t c);
+static bool GPS_newFrame(char c);
+static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing);
+
+/*-----------------------------------------------------------
+ *
+ * GPS low level routines
+ *
+ *-----------------------------------------------------------*/
+
+void USART2_IRQHandler(void)
+{
+    if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
+        GPS_NewData(USART_ReceiveData(USART2));
+    }
+}
+
+static void uart2Init(void)
+{
+    NVIC_InitTypeDef NVIC_InitStructure;
+    GPIO_InitTypeDef GPIO_InitStructure;
+    USART_InitTypeDef USART_InitStructure;
+    
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
+
+    NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+    NVIC_Init(&NVIC_InitStructure);
+
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+    GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+    USART_InitStructure.USART_BaudRate = 9600;
+    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_HardwareFlowControl = USART_HardwareFlowControl_None;
+    USART_Init(USART2, &USART_InitStructure);
+
+    USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
+    USART_Cmd(USART2, ENABLE);
+}
+
+void gpsInit(void)
+{
+    uart2Init();
+    sensorsSet(SENSOR_GPS);
+}
+
+/*-----------------------------------------------------------
+ *
+ * Multiwii GPS code
+ *
+ *-----------------------------------------------------------*/
+
+static void GPS_NewData(uint16_t c)
+{
+    if (GPS_newFrame(c)) {
+        if (GPS_update == 1)
+            GPS_update = 0;
+        else
+            GPS_update = 1;
+        if (GPS_fix == 1 && GPS_numSat > 3) {
+            if (GPS_fix_home == 0) {
+                GPS_fix_home = 1;
+                GPS_latitude_home = GPS_latitude;
+                GPS_longitude_home = GPS_longitude;
+            }
+            if (GPSModeHold == 1)
+                GPS_distance(GPS_latitude_hold, GPS_longitude_hold, GPS_latitude, GPS_longitude, &GPS_distanceToHold, &GPS_directionToHold);
+            else
+                GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome);
+        }
+    }
+}
+
+/* this is an equirectangular approximation to calculate distance and bearing between 2 GPS points (lat/long)
+   it's much more faster than an exact calculation
+   the error is neglectible for few kilometers assuming a constant R for earth
+   input: lat1/long1 <-> lat2/long2      unit: 1/100000 degree
+   output: distance in meters, bearing in degrees
+*/
+static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing)
+{
+    float dLat = (lat2 - lat1); // difference of latitude in 1/100000 degrees
+    float dLon = (lon2 - lon1) * cos(lat1 * (PI / 180 / 100000.0));     // difference of longitude in 1/100000 degrees
+    *dist = 6372795 / 100000.0 * PI / 180 * (sqrt(sq(dLat) + sq(dLon)));
+    *bearing = 180 / PI * (atan2(dLon, dLat));
+}
+
+/* The latitude or longitude is coded this way in NMEA frames
+  dm.m   coded as degrees + minutes + minute decimal
+  Where:
+    - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
+    - m is always 2 char long
+    - m can be 1 or more char long
+  This function converts this format in a unique unsigned long where 1 degree = 100 000
+*/
+static uint32_t GPS_coord_to_degrees(char *s)
+{
+    char *p, *d = s;
+    uint32_t sec, m = 1000;
+    uint16_t min, dec = 0;
+
+    if (!*s)
+        return 0;
+    for (p = s; *p != 0; p++) {
+        if (d != s) {
+            *p -= '0';
+            dec += *p * m;
+            m /= 10;
+        }
+        if (*p == '.')
+            d = p;
+    }
+    m = 10000;
+    min = *--d - '0';
+    min += (*--d - '0') * 10;
+    sec = (m * min + dec) / 6;
+    while (d != s) {
+        m *= 10;
+        *--d -= '0';
+        sec += *d * m;
+    }
+    return sec;
+}
+
+// helper functions 
+static uint16_t grab_fields(char *src, uint8_t mult)
+{                               // convert string to uint16
+    uint8_t i;
+    uint16_t tmp = 0;
+    for (i = 0; src[i] != 0; i++) {
+        if (src[i] == '.') {
+            i++;
+            if (mult == 0)
+                break;
+            else
+                src[i + mult] = 0;
+        }
+        tmp *= 10;
+        if (src[i] >= '0' && src[i] <= '9')
+            tmp += src[i] - '0';
+    }
+    return tmp;
+}
+
+static uint8_t hex_c(uint8_t n)
+{                               // convert '0'..'9','A'..'F' to 0..15
+    n -= '0';
+    if (n > 9)
+        n -= 7;
+    n &= 0x0F;
+    return n;
+}
+
+/* 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
+   Here we use only the following data :
+     - latitude
+     - longitude
+     - GPS fix is/is not ok
+     - GPS num sat (4 is enough to be +/- reliable)
+     // added by Mis
+     - GPS altitude (for OSD displaying)
+     - GPS speed (for OSD displaying)
+*/
+#define FRAME_GGA  1
+#define FRAME_RMC  2
+
+static bool GPS_newFrame(char c)
+{
+    uint8_t frameOK = 0;
+    static uint8_t param = 0, offset = 0, parity = 0;
+    static char string[15];
+    static uint8_t checksum_param, frame = 0;
+
+    if (c == '$') {
+        param = 0;
+        offset = 0;
+        parity = 0;
+    } else if (c == ',' || c == '*') {
+        string[offset] = 0;
+        if (param == 0) {       //frame identification
+            frame = 0;
+            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
+                frame = FRAME_GGA;
+            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
+                frame = FRAME_RMC;
+        } else if (frame == FRAME_GGA) {
+            if (param == 2) {
+                GPS_latitude = GPS_coord_to_degrees(string);
+            } else if (param == 3 && string[0] == 'S')
+                GPS_latitude = -GPS_latitude;
+            else if (param == 4) {
+                GPS_longitude = GPS_coord_to_degrees(string);
+            } else if (param == 5 && string[0] == 'W')
+                GPS_longitude = -GPS_longitude;
+            else if (param == 6) {
+                GPS_fix = string[0] > '0';
+            } else if (param == 7) {
+                GPS_numSat = grab_fields(string, 0);
+            } else if (param == 9) {
+                GPS_altitude = grab_fields(string, 0);
+            }                   // altitude in meters added by Mis
+        } else if (frame == FRAME_RMC) {
+            if (param == 7) {
+                GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L;
+            }                   // speed in cm/s added by Mis
+        }
+        param++;
+        offset = 0;
+        if (c == '*')
+            checksum_param = 1;
+        else
+            parity ^= c;
+    } else if (c == '\r' || c == '\n') {
+        if (checksum_param) {   //parity checksum
+            uint8_t checksum = hex_c(string[0]);
+            checksum <<= 4;
+            checksum += hex_c(string[1]);
+            if (checksum == parity)
+                frameOK = 1;
+        }
+        checksum_param = 0;
+    } else {
+        if (offset < 15)
+            string[offset++] = c;
+        if (!checksum_param)
+            parity ^= c;
+    }
+    return frameOK && (frame == FRAME_GGA);
+}

src/main.c

@@ -72,6 +72,10 @@ int main(void)
     if (feature(FEATURE_VBAT))
         batteryInit();
 
+    // Optional GPS - available only when using PPM, otherwise required pins won't be usable
+    if (feature(FEATURE_PPM) && feature(FEATURE_GPS))
+       gpsInit();
+
     previousTime = micros();
     calibratingG = 400;
 #if defined(POWERMETER)

src/mw.c

@@ -49,6 +49,7 @@ volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502
 // **********************
 int32_t GPS_latitude, GPS_longitude;
 int32_t GPS_latitude_home, GPS_longitude_home;
+int32_t GPS_latitude_hold, GPS_longitude_hold;
 uint8_t GPS_fix, GPS_fix_home = 0;
 uint8_t GPS_numSat;
 uint16_t GPS_distanceToHome;    // in meters
@@ -122,7 +123,7 @@ void annexCode(void)
     if (rcData[THROTTLE] < 1500) {
         prop2 = 100;
     } else if (rcData[THROTTLE] < 2000) {
-        prop2 = 100 - (uint16_t) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500;
+        prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - 1500) / 500;
     } else {
         prop2 = 100 - cfg.dynThrPID;
     }
@@ -139,7 +140,7 @@ void annexCode(void)
         if (axis != 2) {        //ROLL & PITCH
             uint16_t tmp2 = tmp / 100;
             rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
-            prop1 = 100 - (uint16_t) cfg.rollPitchRate *tmp / 500;
+            prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
             prop1 = (uint16_t) prop1 *prop2 / 100;
         } else {                //YAW
             rcCommand[axis] = tmp;
@@ -160,7 +161,6 @@ void annexCode(void)
         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
         rcCommand[PITCH] = rcCommand_PITCH;
     }
-
 #if defined(POWERMETER_HARD)
     if (!(++psensorTimer % PSENSORFREQ)) {
         pMeterRaw = analogRead(PSENSORPIN);
@@ -187,18 +187,17 @@ void annexCode(void)
         if (rcOptions[BOXBEEPERON]) {   // unconditional beeper on via AUXn switch 
             buzzerFreq = 7;
         } else if (((vbat > batteryWarningVoltage)
-    #if defined(POWERMETER)
+#if defined(POWERMETER)
                     && ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0))
-    #endif
+#endif
-                   ) || (vbat < cfg.vbatmincellvoltage))
+                   ) || (vbat < cfg.vbatmincellvoltage)) {      //VBAT ok AND powermeter ok, buzzer off
-        {                           //VBAT ok AND powermeter ok, buzzer off
             buzzerFreq = 0;
             buzzerState = 0;
             BEEP_OFF;
-    #if defined(POWERMETER)
+#if defined(POWERMETER)
         } else if (pMeter[PMOTOR_SUM] > pAlarm) {       // sound alarm for powermeter
             buzzerFreq = 4;
-    #endif
+#endif
         } else
             buzzerFreq = 4;     // low battery
         if (buzzerFreq) {
@@ -475,8 +474,9 @@ void loop(void)
             }
         }
 
-        for(i = 0; i < CHECKBOXITEMS; i++) {
+        for (i = 0; i < CHECKBOXITEMS; i++) {
-            rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
+            rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i])
+                || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
         }
 
         //note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
@@ -525,7 +525,8 @@ void loop(void)
             } else
                 headFreeMode = 0;
         }
-#if defined(GPS)
+
+        if (sensors(SENSOR_GPS)) {
             if (rcOptions[BOXGPSHOME]) {
                 GPSModeHome = 1;
             } else
@@ -539,7 +540,8 @@ void loop(void)
             } else {
                 GPSModeHold = 0;
             }
-#endif
+        }
+
         if (rcOptions[BOXPASSTHRU]) {
             passThruMode = 1;
         } else
@@ -564,7 +566,7 @@ void loop(void)
             break;
         case 3:
             taskOrder++;
-#if GPS
+#if 0                           // GPS - not used as we read gps data in interrupt mode
             GPS_NewData();
 #endif
             break;
@@ -603,14 +605,16 @@ void loop(void)
             rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
         }
     }
-#if GPS
+
-    uint16_t GPS_dist;
+    if (sensors(SENSOR_GPS)) {
-    int16_t  GPS_dir;
+        uint16_t GPS_dist = 0;
+        int16_t GPS_dir = 0;
 
         if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
             GPS_angle[ROLL] = 0;
             GPS_angle[PITCH] = 0;
         } else {
+            float radDiff;
             if (GPSModeHome == 1) {
                 GPS_dist = GPS_distanceToHome;
                 GPS_dir = GPS_directionToHome;
@@ -619,12 +623,11 @@ void loop(void)
                 GPS_dist = GPS_distanceToHold;
                 GPS_dir = GPS_directionToHold;
             }
-        float radDiff = (GPS_dir - heading) * 0.0174533f;
+            radDiff = (GPS_dir - heading) * 0.0174533f;
             GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10);     // with P=5.0, a distance of 1 meter = 0.5deg inclination
             GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10);    // max inclination = D deg
         }
-#endif
+    }
-
     //**** PITCH & ROLL & YAW PID ****    
     for (axis = 0; axis < 3; axis++) {
         if (accMode == 1 && axis < 2) { //LEVEL MODE

src/mw.h

@@ -216,10 +216,11 @@ extern uint8_t baroMode;
 extern uint16_t intPowerMeterSum, intPowerTrigger1;
 extern int32_t GPS_latitude, GPS_longitude;
 extern int32_t GPS_latitude_home, GPS_longitude_home;
+extern int32_t GPS_latitude_hold, GPS_longitude_hold;
 extern uint8_t GPS_fix, GPS_fix_home;
 extern uint8_t GPS_numSat;
-extern uint16_t GPS_distanceToHome;
+extern uint16_t GPS_distanceToHome, GPS_distanceToHold;
-extern int16_t GPS_directionToHome;
+extern int16_t GPS_directionToHome, GPS_directionToHold;
 extern uint8_t GPS_update;
 extern uint8_t GPSModeHome;
 extern uint8_t GPSModeHold;
@@ -278,3 +279,6 @@ uint32_t featureMask(void);
 
 // cli
 void cliProcess(void);
+
+// gps
+void gpsInit(void);

src/serial.c

@@ -152,7 +152,7 @@ void serialCom(void)
             for (i = 0; i < 8; i++)
                 serialize16(rcData[i]);
             serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
-            serialize8(accMode | baroMode << 1 | magMode << 2 | (GPSModeHome | GPSModeHold) << 3);
+            serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4);
 #if defined(LOG_VALUES)
             serialize16(cycleTimeMax);
             cycleTimeMax = 0;