diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
index f9f1767cf..4f114caf0 100644
--- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
+++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
@@ -61,6 +61,7 @@
 #include "positionactual.h"
 #include "homelocation.h"
 #include "ahrscalibration.h"
+#include "CoordinateConversions.h"
 
 #include "pios_opahrs.h" // library for OpenPilot AHRS access functions
 #include "pios_opahrs_proto.h"
@@ -363,15 +364,27 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
   PositionActualGet(&data);
   HomeLocationData home;
   HomeLocationGet(&home);
-  
-  
+
   update->gps.updated = 1;
-  update->gps.NED[0] = 0;
-  update->gps.NED[1] = 0;
-  update->gps.NED[2] = 0;
-  update->gps.groundspeed = 0;
-  update->gps.heading = 0;
-  update->gps.quality = 0;
+
+  if(home.RNE[0] == 0 && home.RNE[1] && home.RNE[2] == 0 && home.RNE[3] == 0)
+  {  
+    update->gps.NED[0] = 0;
+    update->gps.NED[1] = 0;
+    update->gps.NED[2] = 0;
+    update->gps.groundspeed = 0;
+    update->gps.heading = 0;
+    update->gps.quality = 0;
+  }
+  else {
+    update->gps.groundspeed = data.Groundspeed;
+    update->gps.heading = data.Heading;
+    update->gps.quality = 0;
+    double LLA[3] = {(double) data.Latitude, (double) data.Longitude, (double) data.Altitude};
+    double ECEF[3] = {(double) home.ECEF[0], (double) home.ECEF[1], (double) home.ECEF[2]};
+    LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, update->gps.NED);
+  }
+
 }
 
 static void process_update(struct opahrs_msg_v1_rsp_update * update)
diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c
index faa0a6ded..8605b5439 100644
--- a/flight/OpenPilot/Modules/GPS/GPS.c
+++ b/flight/OpenPilot/Modules/GPS/GPS.c
@@ -185,16 +185,13 @@ static void setHomeLocation(PositionActualData * gpsData)
   // Compute home ECEF coordinates and the rotation matrix into NED
   double LLA[3] = {(double) home.Latitude / 10e6, (double) home.Longitude / 10e6, (double) home.Altitude};
   double ECEF[3];
-  float RNE[3][3];
-  RneFromLLA(LLA, RNE);
+  RneFromLLA(LLA, (float (*)[3]) home.RNE);
   LLA2ECEF(LLA, ECEF);
   // TODO: Currently UAVTalk only supports float but these conversions use double
   // need to find out if they require that precision and if so extend UAVTAlk
   home.ECEF[0] = ECEF[0];
   home.ECEF[1] = ECEF[1];
   home.ECEF[2] = ECEF[2];
-  // Can't figure out how to directly cast home.RNE (float *) to a float[3][3]
-  memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0]));
   
   // Compute magnetic flux direction at home location
   WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]);