AP_GPS: Swift Binary Protocol SINGLE-POINT-POSITIONING Driver

This is the SBP driver for Piksi, Lodestar and other forthcoming Swift Navigation GPSes.
This driver currently implements three things:
- Implements a lightweight SBP protocol detection system
- Implements Piksi as a single-point-positioning GPS (same as ublox/others)
- Implements hardware logging of GPS health and baseline messages

Forthcoming iin future updates:
- Need to implement the RTK functionality

libraries/AP_GPS/AP_GPS.cpp

@@ -27,14 +27,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
     // @Param: TYPE
     // @DisplayName: GPS type
     // @Description: GPS type
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol
     AP_GROUPINFO("TYPE",    0, AP_GPS, _type[0], 1),
 
 #if GPS_MAX_INSTANCES > 1
     // @Param: TYPE2
     // @DisplayName: 2nd GPS type
     // @Description: GPS type of 2nd GPS
-    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
+    // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol
     AP_GROUPINFO("TYPE2",   1, AP_GPS, _type[1], 0),
 #endif
 
@@ -60,7 +60,7 @@ void AP_GPS::init(DataFlash_Class *dataflash)
 }
 
 // baudrates to try to detect GPSes with
-const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U};
+const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9600U};
 
 // initialisation blobs to send to the GPS to try to get it into the
 // right mode
@@ -132,8 +132,9 @@ AP_GPS::detect_instance(uint8_t instance)
 		if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) {
 			dstate->last_baud = 0;
 		}
-		uint16_t baudrate = pgm_read_word(&_baudrates[dstate->last_baud]);
+		uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]);
 		port->begin(baudrate, 256, 16);		
+        Debug("Switching to GPS Baudrate %d", baudrate);
 		dstate->last_baud_change_ms = now;
         send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
     }
@@ -150,7 +151,7 @@ AP_GPS::detect_instance(uint8_t instance)
           for.
         */
         if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
-            pgm_read_word(&_baudrates[dstate->last_baud]) >= 38400 && 
+            pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 && 
             AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
             hal.console->print_P(PSTR(" ublox "));
             new_gps = new AP_GPS_UBLOX(*this, state[instance], port);
@@ -165,6 +166,11 @@ AP_GPS::detect_instance(uint8_t instance)
 			hal.console->print_P(PSTR(" MTK "));
 			new_gps = new AP_GPS_MTK(*this, state[instance], port);
 		}
+        else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
+                 AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
+            hal.console->print_P(PSTR(" SBP "));
+            new_gps = new AP_GPS_SBP(*this, state[instance], port);
+        }
 #if !defined( __AVR_ATmega1280__ )
 		// save a bit of code space on a 1280
 		else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&

libraries/AP_GPS/AP_GPS.h

@@ -35,6 +35,13 @@
 #define GPS_MAX_INSTANCES 1
 #endif
 
+#define GPS_DEBUGGING 0
+#if GPS_DEBUGGING
+ # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
 class DataFlash_Class;
 class AP_GPS_Backend;
 
@@ -65,7 +72,8 @@ public:
         GPS_TYPE_MTK19 = 4,
         GPS_TYPE_NMEA  = 5,
         GPS_TYPE_SIRF  = 6,
-        GPS_TYPE_HIL   = 7
+        GPS_TYPE_HIL   = 7,
+        GPS_TYPE_SBP   = 8
     };
 
     /// GPS status codes
@@ -294,6 +302,7 @@ private:
         struct MTK19_detect_state mtk19_detect_state;
         struct SIRF_detect_state sirf_detect_state;
         struct NMEA_detect_state nmea_detect_state;
+        struct SBP_detect_state sbp_detect_state;
     } detect_state[GPS_MAX_INSTANCES];
 
     struct {
@@ -301,7 +310,7 @@ private:
         uint16_t remaining;
     } initblob_state[GPS_MAX_INSTANCES];
 
-    static const uint16_t  _baudrates[];
+    static const uint32_t  _baudrates[];
     static const prog_char _initialisation_blob[];
 
     void detect_instance(uint8_t instance);
@@ -314,5 +323,6 @@ private:
 #include <AP_GPS_MTK19.h>
 #include <AP_GPS_NMEA.h>
 #include <AP_GPS_SIRF.h>
+#include <AP_GPS_SBP.h>
 
 #endif // __AP_GPS_H__

libraries/AP_GPS/GPS_detect_state.h

@@ -53,3 +53,18 @@ struct UBLOX_detect_state {
     uint8_t step;
     uint8_t ck_a, ck_b;
 };
+
+struct SBP_detect_state {
+    enum {
+        WAITING = 0,
+        GET_TYPE = 1,
+        GET_SENDER = 2,
+        GET_LEN = 3,
+        GET_MSG = 4,
+        GET_CRC = 5
+    } state:8;
+    uint8_t n_read;
+    uint8_t msg_len;
+    uint16_t crc_so_far;
+    uint16_t crc;
+};