diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 19f9be4656..afb47d803c 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 19f9be46560047cb3a2586abe22ab6c2433cba7e
+Subproject commit afb47d803cbccfe6d3fde40a22282e2ee264ec09
diff --git a/sw/ground_segment/misc/ublox2ivy.c b/sw/ground_segment/misc/ublox2ivy.c
index 508c3bc3aa..3bfe1ef708 100644
--- a/sw/ground_segment/misc/ublox2ivy.c
+++ b/sw/ground_segment/misc/ublox2ivy.c
@@ -33,6 +33,7 @@
 #include <netinet/in.h>
 #include <pthread.h>
 #include <string.h>
+#include <math.h>
 #include <fcntl.h>
 #include <termios.h>
 #include <glib.h>
@@ -41,6 +42,7 @@
 #include "ublox2ivy.h"
 
 #define UDP_BUFFER_SIZE   1024
+#define NAV_RELPOSNED_VERSION   0x01
 
 /* Endpoints */
 struct endpoint_udp_t {
@@ -79,7 +81,8 @@ static bool verbose = false;
 static struct endpoint_t gps_ep;
 static struct gps_ubx_t gps_ubx;
 static struct gps_rtcm_t gps_rtcm;
-static uint8_t ac_id = 1;
+static uint8_t ac_id = 0;
+static float ground_heading = NAN;
 
 void packet_handler(void *ep, uint8_t *data, uint16_t len);
 
@@ -598,7 +601,7 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
           IvySendMsg("ground FLIGHT_PARAM GCS %f %f %f %f %f %f %f %f %f %f %f %d %f",
                 0.0, // roll,
                 0.0, // pitch,
-                0.0, // heading
+                ground_heading, // heading
                 lat,
                 lon,
                 gSpeed,
@@ -610,7 +613,7 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
                 iTOW, // itow
                 0.0); // airspeed
 
-            IvySendMsg("ground TARGET_POS %d %d %d %d %d %f %f %f",
+            IvySendMsg("ground TARGET_POS %d %d %d %d %d %f %f %f %f",
                 ac_id,
                 ac_id,
                 (int)(lat * 1e7),
@@ -618,7 +621,29 @@ void packet_handler(void *ep, uint8_t *data, uint16_t len) {
                 (int)(alt * 1000),
                 gSpeed,
                 -velD,
-                headMot);
+                headMot,
+                ground_heading);
+          break;
+        }
+        case UBX_NAV_RELPOSNED_ID: {
+          /* Get the heading of the RTK base station */
+          uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf);
+          if(version > NAV_RELPOSNED_VERSION)
+            break;
+
+          uint8_t flags       = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
+          uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1);
+          float relpos_heading = UBX_NAV_RELPOSNED_relPosHeading(gps_ubx.msg_buf) * 1e-5f;
+          float relpos_dist = UBX_NAV_RELPOSNED_relPosLength(gps_ubx.msg_buf) * 1e-2f;
+
+          if(verbose) printf("Got relpos %d %f %f\r\n", flags, relpos_heading, relpos_dist);
+
+          if(relPosValid) {
+            ground_heading = relpos_heading;
+          } else {
+            ground_heading = NAN;
+          }
+          
           break;
         }
         default:
@@ -659,6 +684,7 @@ int main(int argc, char** argv) {
 
   /* Arguments options and usage information */
   static struct option long_options[] = {
+    {"ac_id", required_argument, NULL, 'i'},
     {"endpoint", required_argument, NULL, 'e'},
     {"help", no_argument, NULL, 'h'},
     {"verbose", no_argument, NULL, 'v'},
@@ -667,14 +693,19 @@ int main(int argc, char** argv) {
   static const char* usage =
     "Usage: %s [options]\n"
     " Options :\n"
+    "   -i --ac_id [aircraft_id]               Aircraft id\n"
     "   -e --endpoint [endpoint_str]           Endpoint address of the GPS\n"
     "   -h --help                              Display this help\n"
     "   -v --verbose                           Print verbose information\n";
 
   int c;
   int option_index = 0;
-  while((c = getopt_long(argc, argv, "e:hv", long_options, &option_index)) != -1) {
+  while((c = getopt_long(argc, argv, "i:e:hv", long_options, &option_index)) != -1) {
     switch (c) {
+      case 'i':
+        ac_id = atoi(optarg);
+        break;
+
       case 'e':
         // Parse the endpoint argument UDP
         if(!strncmp(optarg, "udp", 3)) {
diff --git a/sw/ground_segment/misc/ublox2ivy.h b/sw/ground_segment/misc/ublox2ivy.h
index d264cede2d..c15f10a025 100644
--- a/sw/ground_segment/misc/ublox2ivy.h
+++ b/sw/ground_segment/misc/ublox2ivy.h
@@ -79,6 +79,30 @@
 #define UBX_NAV_PVT_magDec(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload+88)|(int16_t)(*((uint8_t*)_ubx_payload+1+88))<<8)
 #define UBX_NAV_PVT_magAcc(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+90)|(uint16_t)(*((uint8_t*)_ubx_payload+1+90))<<8)
 
+/* UBX RELPOSNED message */
+#define UBX_NAV_RELPOSNED_ID 0x3C
+#define UBX_NAV_RELPOSNED_version(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+0))
+#define UBX_NAV_RELPOSNED_reserved1(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+1))
+#define UBX_NAV_RELPOSNED_refStationId(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+2)|(uint16_t)(*((uint8_t*)_ubx_payload+1+2))<<8)
+#define UBX_NAV_RELPOSNED_iTOW(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+4)|(uint32_t)(*((uint8_t*)_ubx_payload+1+4))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
+#define UBX_NAV_RELPOSNED_relPosN(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+8)|(int32_t)(*((uint8_t*)_ubx_payload+1+8))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
+#define UBX_NAV_RELPOSNED_relPosE(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+12)|(int32_t)(*((uint8_t*)_ubx_payload+1+12))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
+#define UBX_NAV_RELPOSNED_relPosD(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+16)|(int32_t)(*((uint8_t*)_ubx_payload+1+16))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
+#define UBX_NAV_RELPOSNED_relPosLength(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+20)|(int32_t)(*((uint8_t*)_ubx_payload+1+20))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
+#define UBX_NAV_RELPOSNED_relPosHeading(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+24)|(int32_t)(*((uint8_t*)_ubx_payload+1+24))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
+#define UBX_NAV_RELPOSNED_reserved2(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+28)|(uint32_t)(*((uint8_t*)_ubx_payload+1+28))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
+#define UBX_NAV_RELPOSNED_relPosHPN(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+32))
+#define UBX_NAV_RELPOSNED_relPosHPE(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+33))
+#define UBX_NAV_RELPOSNED_relPosHPD(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+34))
+#define UBX_NAV_RELPOSNED_relPosHPLength(_ubx_payload) (int8_t)(*((uint8_t*)_ubx_payload+35))
+#define UBX_NAV_RELPOSNED_accN(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+36)|(uint32_t)(*((uint8_t*)_ubx_payload+1+36))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+36))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+36))<<24)
+#define UBX_NAV_RELPOSNED_accE(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+40)|(uint32_t)(*((uint8_t*)_ubx_payload+1+40))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+40))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+40))<<24)
+#define UBX_NAV_RELPOSNED_accD(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+44)|(uint32_t)(*((uint8_t*)_ubx_payload+1+44))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+44))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+44))<<24)
+#define UBX_NAV_RELPOSNED_accLength(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+48)|(uint32_t)(*((uint8_t*)_ubx_payload+1+48))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+48))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+48))<<24)
+#define UBX_NAV_RELPOSNED_accHeading(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+52)|(uint32_t)(*((uint8_t*)_ubx_payload+1+52))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+52))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+52))<<24)
+#define UBX_NAV_RELPOSNED_reserved3(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+56)|(uint32_t)(*((uint8_t*)_ubx_payload+1+56))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+56))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+56))<<24)
+#define UBX_NAV_RELPOSNED_flags(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+60)|(uint32_t)(*((uint8_t*)_ubx_payload+1+60))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+60))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+60))<<24)
+
 struct gps_ubx_t {
   bool msg_available;
   uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned));
diff --git a/sw/ground_segment/tmtc/gpsd2ivy.c b/sw/ground_segment/tmtc/gpsd2ivy.c
index d9cff37dd8..e4a7ce1ddc 100644
--- a/sw/ground_segment/tmtc/gpsd2ivy.c
+++ b/sw/ground_segment/tmtc/gpsd2ivy.c
@@ -144,7 +144,7 @@ static void update_gps(struct gps_data_t *gpsdata,
 
 
         if(strcmp(ac, "NONE") != 0) {
-            IvySendMsg("%s TARGET_POS %s %s %d %d %d %f %f %f", "0", "0", ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(fix_altitude* 1000), fix_speed, fix_climb, fix_track);
+            IvySendMsg("%s TARGET_POS %s %s %d %d %d %f %f %f %f", "0", "0", ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(fix_altitude* 1000), fix_speed, fix_climb, fix_track, NAN);
             if (verbose)
                 printf("sending TARGET_POS for aircraft %s\n", ac);
         }