diff --git a/conf/messages.xml b/conf/messages.xml
index c615ceaf8c..936218ec28 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -178,6 +178,16 @@
     <field name="nb_msg" type="uint32"></field>
     <field name="nb_err" type="uint32"></field>
   </message>
+
+  <message name="SVINFO" id="20">
+     <field name="chn" type="uint8"/>
+     <field name="SVID" type="uint8"/>
+     <field name="Flags" type="uint8"/>
+     <field name="QI" type="int8"/>
+     <field name="CNO" type="uint8" unit="dbHz"/>
+     <field name="Elev" type="int8" unit="deg"/>
+     <field name="Azim" type="int16" unit="deg"/>
+  </message>
 </class>
 
 <class name="telemetry_fbw" ID="0X41">
diff --git a/conf/ubx.xml b/conf/ubx.xml
index 1ac484b8bb..59421f5c2f 100644
--- a/conf/ubx.xml
+++ b/conf/ubx.xml
@@ -65,4 +65,3 @@
  </class>
 
 </ubx>
-
diff --git a/sw/airborne/autopilot/gps.h b/sw/airborne/autopilot/gps.h
index d88480d9b5..a85e9b6835 100644
--- a/sw/airborne/autopilot/gps.h
+++ b/sw/airborne/autopilot/gps.h
@@ -50,6 +50,23 @@ extern volatile uint8_t gps_msg_received;
 extern bool_t gps_pos_available;
 extern uint8_t gps_nb_ovrn;
 
+#define NB_CHANNELS 16
+
+/** Number of scanned satellites */
+extern uint8_t gps_nb_channels;
+
+/** Space Vehicle Information */
+struct svinfo {
+  uint8_t svid;
+  uint8_t flags;
+  uint8_t qi;
+  uint8_t cno;
+  int8_t elev;
+  int16_t azim;
+};
+
+extern struct svinfo gps_svinfos[NB_CHANNELS];
+
 #ifdef UBX
 #include "ubx.h"
 #endif
diff --git a/sw/airborne/autopilot/gps_ubx.c b/sw/airborne/autopilot/gps_ubx.c
index ea6ae46488..375a1b496b 100644
--- a/sw/airborne/autopilot/gps_ubx.c
+++ b/sw/airborne/autopilot/gps_ubx.c
@@ -93,6 +93,9 @@ void gps_init( void ) {
   ubx_status = UNINIT;
 }
 
+struct svinfo gps_svinfos[NB_CHANNELS];
+uint8_t gps_nb_channels;
+
 void parse_gps_msg( void ) {
   if (ubx_class == UBX_NAV_ID) {
     if (ubx_id == UBX_NAV_POSUTM_ID) {
@@ -113,6 +116,17 @@ void parse_gps_msg( void ) {
       
       
       gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
+    } else if (ubx_id == UBX_NAV_SVINFO_ID) {
+      gps_nb_channels = UBX_NAV_SVINFO_NCH(ubx_msg_buf);
+      uint8_t i;
+      for(i = 0; i < Min(gps_nb_channels, NB_CHANNELS); i++) {
+	gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
+	gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
+	gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
+	gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
+	gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
+	gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
+      }
     }
   }
 #ifdef SIMUL
diff --git a/sw/airborne/autopilot/main.c b/sw/airborne/autopilot/main.c
index f478218ab3..34e1be9447 100644
--- a/sw/airborne/autopilot/main.c
+++ b/sw/airborne/autopilot/main.c
@@ -524,6 +524,11 @@ void use_gps_pos( void ) {
   DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_fcourse, &gps_falt, &gps_fspeed,&gps_fclimb, &gps_ftow, &gps_utm_zone);
   estimator_update_state_gps();
   SEND_RAD_OF_IR();
+  
+  static uint8_t i;
+  if (i == gps_nb_channels) i = 0;
+  DOWNLINK_SEND_SVINFO(&i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim);
+
   if (!estimator_flight_time && (estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF)) {
     estimator_flight_time = 1;
     launch = TRUE; /* Not set in non auto launch */
diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c
index e7a175088d..8828900c81 100644
--- a/sw/simulator/sim_ap.c
+++ b/sw/simulator/sim_ap.c
@@ -8,6 +8,7 @@
 #include "link_autopilot.h"
 #include "autopilot.h"
 #include "estimator.h"
+#include "gps.h"
 
 #include <caml/mlvalues.h>
 #include <caml/memory.h>
@@ -20,6 +21,10 @@ bool_t launch;
 bool_t link_fbw_receive_valid;
 uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
 
+struct svinfo gps_svinfos[NB_CHANNELS];
+uint8_t gps_nb_channels;
+
+
 uint8_t ac_id;
 
 struct inter_mcu_msg from_fbw, to_fbw;
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index d639414fa7..4091db84aa 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -190,6 +190,7 @@ let output_vmode x wp last_wp =
 	    else sprintf "waypoints[%s].a" wp in
 	lprintf "desired_altitude = %s;\n" alt;
 	lprintf "pre_climb = 0.;\n"
+    | "xyz" -> () (** Handled in Goto3D() *)
     | "glide" ->
 	lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n";
 	lprintf "glide_to(%s, %s);\n" last_wp wp
@@ -348,6 +349,8 @@ let rec print_stage = fun index_of_waypoints x ->
 	stage ();
 	let r = try parsed_attrib  x "radius" with _ -> "100" in
 	lprintf "Goto3D(%s)\n" r;
+	let x = ExtXml.subst_attrib "vmode" "xyz" x in
+	ignore (output_vmode x "" ""); (** To handle "pitch" *)
 	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
     | "circle" ->