GPS: Official Swift Binary Protocol GPS Driver v3 for Piksi GPS

DRIVER FEATURES:
- All logic for RTK vs Normal now lives inside Piksi
- Supports observation uplink through telem radio
- Supports full SBP packet logging
- Reports high-rate green blinking to indicate RTK lock.
- Switchable to accept only Float or Integer RTK locks.

THIS REQUIRES PIKSI FIRMWARE v0.14 OR HIGHER
- Uses Piksi's new Pseudo-Absolute-Positioning mode
- Onboard Piksi must have Pseudo-Absolute mode enabled
- Ground Station Piksi must have a surveyed location in settings
- Ground Station Piksi must send its location to Onboard Piksi.

NEXT STEPS REQUIRED:
- EKF needs to take higher accuracy GPS into account
- EKF needs to take GPS RTK height into account
- GCS needs to support sending SBP observation packets
--- MAVProxy patch forthcoming

libraries/AP_GPS/AP_GPS.cpp

@@ -79,6 +79,25 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100),
 
+#if GPS_MAX_INSTANCES > 1
+
+    // @Param: INJECT_TO
+    // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets
+    // @Description: The GGS can send raw serial packets to inject data to multiple GPSes.
+    // @Values: 0,1: send to specified instance. 127: broadcast
+    AP_GROUPINFO("INJECT_TO",   7, AP_GPS, _inject_to, 127),
+
+#endif
+
+#if GPS_RTK_AVAILABLE
+    // @Param: SBP_LOGMASK
+    // @DisplayName: Swift Binary Protocol Logging Mask
+    // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged
+    // @Values: 0x0000 for none, 0xFFFF for all, 0xFF00 for external only.
+    // @User: Advanced
+    AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
+#endif
+
     AP_GROUPEND
 };
 
@@ -251,34 +270,6 @@ found_gps:
 	}
 }
 
-bool 
-AP_GPS::can_calculate_base_pos(void)
-{
-#if GPS_RTK_AVAILABLE
-    for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
-        if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
-            return true;
-        }
-    }
-#endif
-    return false;
-}
-
-/*
-    Tells the underlying GPS drivers to capture its current position as home.
- */
-void 
-AP_GPS::calculate_base_pos(void) 
-{
-#if GPS_RTK_AVAILABLE
-    for (uint8_t i = 0; i<GPS_MAX_INSTANCES; i++) {
-        if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
-            drivers[i]->calculate_base_pos(); 
-        }
-    }
-#endif
-}
-
 AP_GPS::GPS_Status 
 AP_GPS::highest_supported_status(uint8_t instance) const
 {
@@ -441,6 +432,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
 void 
 AP_GPS::lock_port(uint8_t instance, bool lock)
 {
+
     if (instance >= GPS_MAX_INSTANCES) {
         return;
     }
@@ -451,6 +443,35 @@ AP_GPS::lock_port(uint8_t instance, bool lock)
     }
 }
 
+    //Inject a packet of raw binary to a GPS
+void 
+AP_GPS::inject_data(uint8_t *data, uint8_t len)
+{
+
+#if GPS_MAX_INSTANCES > 1
+
+    //Support broadcasting to all GPSes.
+    if (_inject_to == 127) {
+        for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
+            inject_data(i, data, len);
+        }
+    } else {
+        inject_data(_inject_to, data, len);
+    }
+
+#else
+    inject_data(0,data,len);
+#endif
+
+}
+
+void 
+AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len)
+{
+    if (instance < GPS_MAX_INSTANCES && drivers[instance] != NULL)
+        drivers[instance]->inject_data(data, len);
+}  
+
 void 
 AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
 {

libraries/AP_GPS/AP_GPS.h

@@ -74,14 +74,6 @@ public:
     /// more) to process incoming data.
     void update(void);
 
-    //True if any of the underlying GPS Drivers are ready to enter
-    //a dgps-based fix beyond 3D lock, such as RTK mode. 
-    bool can_calculate_base_pos(void);
-
-    //Allows the underlying GPS Drivers to enter a differential lock
-    //Might cause a position jump, thus only do this on the ground.
-    void calculate_base_pos(void);
-
     // GPS driver types
     enum GPS_Type {
         GPS_TYPE_NONE  = 0,
@@ -336,6 +328,8 @@ public:
 #if GPS_MAX_INSTANCES > 1
     AP_Int8 _auto_switch;
     AP_Int8 _min_dgps;
+    AP_Int16 _sbp_logmask;
+    AP_Int8 _inject_to;
 #endif
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;
@@ -347,6 +341,10 @@ public:
     // lock out a GPS port, allowing another application to use the port
     void lock_port(uint8_t instance, bool locked);
 
+    //Inject a packet of raw binary to a GPS
+    void inject_data(uint8_t *data, uint8_t len);
+    void inject_data(uint8_t instance, uint8_t *data, uint8_t len);
+
     //MAVLink Status Sending
     void send_mavlink_gps_raw(mavlink_channel_t chan);
 #if GPS_MAX_INSTANCES > 1    

libraries/AP_GPS/AP_GPS_SBP.h

@@ -18,59 +18,31 @@
 //  Swift Navigation SBP GPS driver for ArduPilot.
 //	Code by Niels Joubert
 //
-//  Swift Binary Protocol format: http://docs.swift-nav.com/libswiftnav
+//  Swift Binary Protocol format: http://docs.swift-nav.com/
+//
 
 #ifndef __AP_GPS_SBP_H__
 #define __AP_GPS_SBP_H__
 
-#if GPS_RTK_AVAILABLE
-
 #include <AP_GPS.h>
 
-
-//OVERALL DESIGN NOTES.
-//  Niels Joubert, April 2014
-//
-//
-// REQUIREMENTS:
-// 1) We need to update the entire state structure "atomically",
-//    which is indicated by returning "true" from read().
-//
-//      - We use sticky bits to track when each part is updated
-//        and return true when all the sticky bits are set
-//
-// 2) We want to minimize memory usage in the detection routine
-//
-//      - We use a stripped-down version of the sbp_parser_state_t struct
-//        that does not bother to decode the message, it just tracks the CRC.
-//
-
 class AP_GPS_SBP : public AP_GPS_Backend
 {
 public:
-	AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
+    AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
-
-    bool can_calculate_base_pos(void);
-
-    void calculate_base_pos(void);
-
-    void invalidate_base_pos(void);
 
     AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; }
 
+    // Methods
     bool read();
 
+    void inject_data(uint8_t *data, uint8_t len);
+
     static bool _detect(struct SBP_detect_state &state, uint8_t data);
 
-    virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
-
-#if GPS_MAX_INSTANCES > 1
-    virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan);
-#endif
-
 private:
 
-    // ************************************************************************
+   // ************************************************************************
     // Swift Navigation SBP protocol types and definitions
     // ************************************************************************
   
@@ -107,13 +79,14 @@ private:
     static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0016;
     static const uint16_t SBP_IAR_STATE_MSGTYPE      = 0x0019;
     
+
     // GPS Time
     struct PACKED sbp_gps_time_t {
         uint16_t wn;     //< GPS week number (unit: weeks)
         uint32_t tow;    //< GPS Time of Week rounded to the nearest ms (unit: ms)
         int32_t ns;      //< Nanosecond remainder of rounded tow (unit: ns)
         uint8_t flags;   //< Status flags (reserved)
-    };
+    }; // 11 bytes
 
     // Dilution of Precision
     struct PACKED sbp_dops_t {
@@ -123,18 +96,7 @@ private:
         uint16_t tdop;   //< Time Dilution of Precision (unit: 0.01)
         uint16_t hdop;   //< Horizontal Dilution of Precision (unit: 0.01)
         uint16_t vdop;   //< Vertical Dilution of Precision (unit: 0.01)
-    };
+    }; // 14 bytes
-
-    // Position solution in absolute Earth Centered Earth Fixed (ECEF) coordinates.
-    struct PACKED sbp_pos_ecef_t {
-        uint32_t tow;       //< GPS Time of Week (unit: ms)
-        double x;           //< ECEF X coordinate (unit: meters)
-        double y;           //< ECEF Y coordinate (unit: meters)
-        double z;           //< ECEF Z coordinate (unit: meters)
-        uint16_t accuracy;  //< Position accuracy estimate (unit: mm)
-        uint8_t n_sats;     //< Number of satellites used in solution
-        uint8_t flags;      //< Status flags
-    };
 
     // Geodetic position solution.
     struct PACKED sbp_pos_llh_t {
@@ -146,41 +108,7 @@ private:
         uint16_t v_accuracy;  //< Vertical position accuracy estimate (unit: mm)
         uint8_t n_sats;       //< Number of satellites used in solution
         uint8_t flags;        //< Status flags
-    };
+    }; // 34 bytes
-
-    // Baseline in Earth Centered Earth Fixed (ECEF) coordinates.
-    struct PACKED sbp_baseline_ecef_t {
-        uint32_t tow;        //< GPS Time of Week (unit: ms)
-        int32_t x;           //< Baseline ECEF X coordinate (unit: mm)
-        int32_t y;           //< Baseline ECEF Y coordinate (unit: mm)
-        int32_t z;           //< Baseline ECEF Z coordinate (unit: mm)
-        uint16_t accuracy;   //< Position accuracy estimate (unit: mm)
-        uint8_t n_sats;      //< Number of satellites used in solution
-        uint8_t flags;       //< Status flags (reserved)
-    };
-
-    // Baseline in local North East Down (NED) coordinates.
-    struct PACKED sbp_baseline_ned_t {
-        uint32_t tow;          //< GPS Time of Week (unit: ms)
-        int32_t n;             //< Baseline North coordinate (unit: mm)
-        int32_t e;             //< Baseline East coordinate  (unit: mm)
-        int32_t d;             //< Baseline Down coordinate  (unit: mm)
-        uint16_t h_accuracy;   //< Horizontal position accuracy estimate (unit: mm)
-        uint16_t v_accuracy;   //< Vertical position accuracy estimate (unit: mm)
-        uint8_t n_sats;        //< Number of satellites used in solution
-        uint8_t flags;         //< Status flags (reserved)
-    };
-
-    //Velocity in Earth Centered Earth Fixed (ECEF) coordinates.
-    struct PACKED sbp_vel_ecef_t {
-        uint32_t tow;          //< GPS Time of Week (unit: ms)
-        int32_t x;             //< Velocity ECEF X coordinate (unit: mm/s)
-        int32_t y;             //< Velocity ECEF Y coordinate (unit: mm/s)
-        int32_t z;             //< Velocity ECEF Z coordinate (unit: mm/s)
-        uint16_t accuracy;     //< Velocity accuracy estimate (unit: mm/s)
-        uint8_t n_sats;        //< Number of satellites used in solution
-        uint8_t flags;         //< Status flags (reserved)
-    };
 
     // Velocity in NED Velocity in local North East Down (NED) coordinates.
     struct PACKED sbp_vel_ned_t {
@@ -192,7 +120,7 @@ private:
         uint16_t v_accuracy;   //< Vertical velocity accuracy estimate (unit: mm/s)
         uint8_t n_sats;        //< Number of satellites used in solution 
         uint8_t flags;         //< Status flags (reserved)
-    };
+    }; // 22 bytes
 
     // Activity and Signal-to-Noise data of a tracking channel on the GPS.
     struct PACKED sbp_tracking_state_t {
@@ -206,74 +134,31 @@ private:
         uint32_t num_hypotheses;
     };
 
-
+    void _sbp_process();
+    void _sbp_process_message();
+    bool _attempt_state_update();
 
     // ************************************************************************
-    // Swift Navigation SBP protocol parsing and processing
+    // Internal Received Messages State
     // ************************************************************************
-
-    //Pulls data from the port, dispatches messages to processing functions
-    //Returns true if a new message was successfully decoded.
-    bool sbp_process();
-
-    bool update_state(bool has_new_message);
-    
-    void update_state_velocity(void);
-
-    //Processes individual messages
-    //When a message is received, it sets a sticky bit that it has updated
-    //itself. This is used to track when a full update of GPS_State has occurred
-    void sbp_process_heartbeat(uint8_t* msg);    
-    void sbp_process_gpstime(uint8_t* msg);
-    void sbp_process_dops(uint8_t* msg);
-    void sbp_process_pos_ecef(uint8_t* msg);
-    void sbp_process_pos_llh(uint8_t* msg);
-    void sbp_process_baseline_ecef(uint8_t* msg);
-    void sbp_process_baseline_ned(uint8_t* msg);
-    void sbp_process_vel_ecef(uint8_t* msg);
-    void sbp_process_vel_ned(uint8_t* msg);
-    void sbp_process_tracking_state(uint8_t* msg, uint8_t len);
-    void sbp_process_iar_state(uint8_t* msg);
-    void sbp_process_startup(uint8_t* msg);
-
-
-    //Internal last-received-messages
-    sbp_pos_llh_t            last_sbp_pos_llh_msg;
-    sbp_vel_ned_t            last_sbp_vel_ned_msg;
-    sbp_baseline_ecef_t      last_sbp_baseline_ecef_msg;
-    sbp_baseline_ned_t       last_sbp_baseline_ned_msg;
-    sbp_tracking_state_t     last_sbp_tracking_state_msg;
-    uint8_t                  last_sbp_tracking_state_msg_num;
-
-    //Tracking GPS health and received time-of-week
-    uint32_t last_baseline_received_ms;
     uint32_t last_heatbeat_received_ms;
-    uint32_t last_tracking_state_ms;
+    uint32_t last_injected_data_ms;
-    int32_t iar_num_hypotheses;
-    uint8_t baseline_recv_rate; //in hertz * 10
 
-    //Sticky bits to track updating of state
+    struct sbp_gps_time_t last_gps_time;
-    bool dgps_corrections_incoming:1;
+    struct sbp_dops_t     last_dops;
-    bool rtk_corrections_incoming:1;
+    struct sbp_pos_llh_t  last_pos_llh_spp;
+    struct sbp_pos_llh_t  last_pos_llh_rtk;
+    struct sbp_vel_ned_t  last_vel_ned;
+    uint32_t              last_iar_num_hypotheses;
 
-    bool has_new_pos_llh:1;
+    uint32_t              last_full_update_tow;
-    bool has_new_vel_ned:1;
+    uint32_t              last_full_update_cpu_ms;
-    bool has_new_baseline_ecef:1;
 
-    //RTK-specific relative-to-absolute positioning
-    bool has_rtk_base_pos:1;
-    Vector3d base_pos_ecef;
-    
     // ************************************************************************
     // Monitoring and Performance Counting
     // ************************************************************************
 
-    uint8_t  pos_msg_counter;
-    uint8_t  vel_msg_counter;
-    uint8_t  baseline_msg_counter;
-    uint8_t  full_update_counter;
     uint32_t crc_error_counter;
-    uint32_t last_healthcheck_millis;
 
     // ************************************************************************
     // Logging to DataFlash
@@ -283,14 +168,10 @@ private:
     static bool logging_started;
 
     void logging_write_headers();
-
+    void logging_log_full_update();
-    void logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz);
+    void logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff);
-    void logging_log_llh(struct sbp_pos_llh_t* p);
+   
-    void logging_log_baseline_ecef(struct sbp_baseline_ecef_t*);      
-    void logging_log_tracking_state(struct sbp_tracking_state_t*, uint8_t num);      
 
 };
 
-#endif //  GPS_RTK_AVAILABLE
+#endif // __AP_GPS_SBP_H__
-
-#endif // __AP_GPS_SBP_H__ 

libraries/AP_GPS/GPS_Backend.h

@@ -37,16 +37,9 @@ public:
     // valid packet from the GPS.
     virtual bool read() = 0;
 
+    virtual void inject_data(uint8_t *data, uint8_t len) { return; }
+
 #if GPS_RTK_AVAILABLE
-
-    // true once an RTK GPS Driver has a converged baseline vector and
-    // absolute single point solution to enter into an RTK Fix. 
-    virtual bool can_calculate_base_pos(void) { return false; };
-
-    // tells a RTK GPS Driver to capture the current single-point solution
-    // and baseline solution as the current home data.
-    virtual void calculate_base_pos(void) {};
-
     // Highest status supported by this GPS. 
     // Allows external system to identify type of receiver connected.
     virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }