Adding RTK functionalities to Solo

ArduCopter/GCS_Mavlink.pde

@@ -1737,6 +1737,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+    case MAVLINK_MSG_ID_GPS_RTCM_DATA:
+	{
+        gps.handle_msg(msg);
+        result = MAV_RESULT_ACCEPTED;
+        break;
+	}
+
 #if HIL_MODE != HIL_MODE_DISABLED
     case MAVLINK_MSG_ID_HIL_STATE:          // MAV ID: 90
     {

libraries/AP_GPS/AP_GPS.cpp

@@ -404,6 +404,25 @@ AP_GPS::update(void)
     AP_Notify::flags.gps_status = state[primary_instance].status;
 }
 
+/*
+  pass along a mavlink message (for MAV type)
+ */
+void
+AP_GPS::handle_msg(const mavlink_message_t *msg)
+{
+    if (msg->msgid == MAVLINK_MSG_ID_GPS_RTCM_DATA) {
+        // pass data to de-fragmenter
+        handle_gps_rtcm_data(msg);
+        return;
+    }
+    uint8_t i;
+    for (i=0; i<num_instances; i++) {
+        if ((drivers[i] != nullptr) && (_type[i] != GPS_TYPE_NONE)) {
+            drivers[i]->handle_msg(msg);
+        }
+    }
+}
+
 /*
   set HIL (hardware in the loop) status for a GPS instance
  */
@@ -459,9 +478,6 @@ AP_GPS::lock_port(uint8_t instance, bool lock)
 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++) {
@@ -470,18 +486,14 @@ AP_GPS::inject_data(uint8_t *data, uint8_t 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)
+    if (instance < GPS_MAX_INSTANCES && drivers[instance] != NULL){
         drivers[instance]->inject_data(data, len);
+	}	
 }  
 
 void 
@@ -568,3 +580,87 @@ AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan)
 }
 #endif
 #endif
+
+/* 
+   re-assemble GPS_RTCM_DATA message
+ */
+void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t *msg)
+{
+    mavlink_gps_rtcm_data_t packet;
+    mavlink_msg_gps_rtcm_data_decode(msg, &packet);
+
+    if (packet.len > MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
+        // invalid packet
+        return;
+    }
+
+    if ((packet.flags & 1) == 0) {
+        // it is not fragmented, pass direct
+        inject_data_all(packet.data, packet.len);
+        return;
+    }
+
+    // see if we need to allocate re-assembly buffer
+    if (rtcm_buffer == nullptr) {
+        rtcm_buffer = (struct rtcm_buffer *)calloc(1, sizeof(*rtcm_buffer));
+        if (rtcm_buffer == nullptr) {
+            // nothing to do but discard the data
+            return;
+        }
+    }
+
+    uint8_t fragment = (packet.flags >> 1U) & 0x03;
+    uint8_t sequence = (packet.flags >> 3U) & 0x1F;
+
+    // see if this fragment is consistent with existing fragments
+    if (rtcm_buffer->fragments_received &&
+        (rtcm_buffer->sequence != sequence ||
+         rtcm_buffer->fragments_received & (1U<<fragment))) {
+        // we have one or more partial fragments already received
+        // which conflict with the new fragment, discard previous fragments
+        memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
+    }
+
+    // add this fragment
+    rtcm_buffer->sequence = sequence;
+    rtcm_buffer->fragments_received |= (1U << fragment);
+
+    // copy the data
+    memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], packet.data, packet.len);
+
+    // when we get a fragment of less than max size then we know the
+    // number of fragments. Note that this means if you want to send a
+    // block of RTCM data of an exact multiple of the buffer size you
+    // need to send a final packet of zero length
+    if (packet.len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
+        rtcm_buffer->fragment_count = fragment+1;
+        rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + packet.len;
+    } else if (rtcm_buffer->fragments_received == 0x0F) {
+        // special case of 4 full fragments
+        rtcm_buffer->fragment_count = 4;
+        rtcm_buffer->total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4;
+    }
+
+
+    // see if we have all fragments
+    if (rtcm_buffer->fragment_count != 0 &&
+        rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) {
+        // we have them all, inject
+        inject_data_all(rtcm_buffer->buffer, rtcm_buffer->total_length);
+        memset(rtcm_buffer, 0, sizeof(*rtcm_buffer));
+    }
+}
+
+/*
+  inject data into all backends
+*/
+void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
+{
+    uint8_t i;
+    for (i=0; i<num_instances; i++) {
+        if ((drivers[i] != nullptr) && (_type[i] != GPS_TYPE_NONE)) {
+            drivers[i]->inject_data(data, len);
+        }
+    }
+
+}

libraries/AP_GPS/AP_GPS.h

@@ -139,6 +139,9 @@ class AP_GPS
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
     };
 
+	// Pass mavlink data to message handlers (for MAV type)
+    void handle_msg(const mavlink_message_t *msg);
+
     // Accessor functions
 
     // return number of active GPS sensors. Note that if the first GPS
@@ -407,6 +410,32 @@ class AP_GPS
 
     void detect_instance(uint8_t instance);
     void update_instance(uint8_t instance);
+
+    /*
+      buffer for re-assembling RTCM data for GPS injection. 
+      The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
+              1 bit for "is fragmented"
+              2 bits for fragment number
+              5 bits for sequence number
+
+      The rtcm_buffer is allocated on first use. Once a block of data
+      is successfully reassembled it is injected into all active GPS
+      backends. This assumes we don't want more than 4*180=720 bytes
+      in a RTCM data block
+     */
+    struct rtcm_buffer {
+        uint8_t fragments_received:4;
+        uint8_t sequence:5;
+        uint8_t fragment_count;
+        uint16_t total_length;
+        uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4];
+    } *rtcm_buffer;
+
+    // re-assemble GPS_RTCM_DATA message
+    void handle_gps_rtcm_data(const mavlink_message_t *msg);
+
+    // inject data into all backends
+    void inject_data_all(const uint8_t *data, uint16_t len);
 };
 
 #include <GPS_Backend.h>

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -503,8 +503,11 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_STATUS fix_status=%u fix_type=%u",
               _buffer.status.fix_status,
               _buffer.status.fix_type);
-        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID && !gps._ublox_no_fix) {
-            if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
+        if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
+            if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
+                (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
+                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+            }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
                 next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
                 next_fix = AP_GPS::GPS_OK_FIX_2D;
@@ -531,8 +534,11 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_SOL fix_status=%u fix_type=%u",
               _buffer.solution.fix_status,
               _buffer.solution.fix_type);
-        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID && !gps._ublox_no_fix) {
-            if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
+        if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
+            if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
+                (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
+                next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
+            }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
                 next_fix = AP_GPS::GPS_OK_FIX_3D;
             }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
                 next_fix = AP_GPS::GPS_OK_FIX_2D;
@@ -824,3 +830,13 @@ AP_GPS_UBLOX::_request_version(void)
 {
     _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2);
 }
+
+void
+AP_GPS_UBLOX::inject_data(const uint8_t *data, uint16_t len)
+{
+    if (port->txspace() > len) {
+        port->write(data, len);
+    } else {
+        Debug("UBX: Not enough TXSPACE");
+    }
+}

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -37,7 +37,7 @@
  * modules are configured with all ubx binary messages off, which
  * would mean we would never detect it.
  */
-#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
+#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,38400,0*24\r\n"
 
 //Configuration Sub-Sections
 #define SAVE_CFG_IO     (1<<0)
@@ -59,6 +59,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
 
+	void inject_data(const uint8_t *data, uint16_t len) override;
+
 private:
     // u-blox UBX protocol essentials
     struct PACKED ubx_header {
@@ -279,7 +281,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         FIX_TIME = 5
     };
     enum ubx_nav_status_bits {
-        NAV_STATUS_FIX_VALID = 1
+        NAV_STATUS_FIX_VALID = 1,
+        NAV_STATUS_DGPS_USED = 2
     };
     enum ubx_hardware_version {
         ANTARIS = 0,

libraries/AP_GPS/GPS_Backend.cpp

@@ -15,6 +15,7 @@
  */
 
 #include <AP_GPS.h>
+#include "GPS_Backend.h"
 
 extern const AP_HAL::HAL& hal;
 

libraries/AP_GPS/GPS_Backend.h

@@ -37,7 +37,7 @@ class AP_GPS_Backend
     // valid packet from the GPS.
     virtual bool read() = 0;
 
-    virtual void inject_data(uint8_t *data, uint8_t len) { return; }
+    virtual void inject_data(const uint8_t *data, uint16_t len) { return; }
 
 #if GPS_RTK_AVAILABLE
     // Highest status supported by this GPS. 
@@ -53,6 +53,8 @@ class AP_GPS_Backend
 
 #endif
 
+    virtual void handle_msg(const mavlink_message_t *msg) { return ; }
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)