diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml
index 65281893a3..bd4bdd0633 100644
--- a/conf/telemetry/default_fixedwing.xml
+++ b/conf/telemetry/default_fixedwing.xml
@@ -26,6 +26,8 @@
       <message name="IMU_GYRO"            period="1.1"/>
       <message name="SURVEY"              period="2.1"/>
       <message name="GPS_SOL"             period="2.0"/>
+      <message name="CAM"                 period="0.5"/>
+      <message name="CAM_POINT"           period="1.0"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"               period="5"/>
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index f295e16397..726ac6f343 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -28,6 +28,8 @@
       <message name="IMU_ACCEL"           period=".8"/>
       <message name="IMU_GYRO"            period=".6"/>
       <message name="IMU_MAG"             period="1.3"/>
+      <message name="CAM"                 period="0.5"/>
+      <message name="CAM_POINT"           period="1.0"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"               period="5"/>
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c
index 47f42b6d83..2b2ada238f 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot.c
@@ -66,7 +66,7 @@ static void send_rc_settings(void) {
 uint8_t rc_settings_mode = 0;
 #endif
 
-static void send_mode(void) {
+void autopilot_send_mode(void) {
   DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice,
       &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
 }
@@ -149,7 +149,7 @@ void autopilot_init(void) {
 
   /* register some periodic message */
   register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
-  register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode);
+  register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", autopilot_send_mode);
   register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink);
   register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
   register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator);
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index 0e0ec52fb1..0dad775893 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -110,6 +110,9 @@ extern bool_t gps_lost;
   (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
 })
 
+/** Send mode over telemetry
+ */
+extern void autopilot_send_mode(void);
 
 /** Power switch control.
  */
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 9b7e5def6a..9e418d14a5 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -387,9 +387,7 @@ static inline void telecommand_task( void ) {
 #endif
   }
   mode_changed |= mcu1_status_update();
-  // FIXME
-  //if ( mode_changed )
-  //  PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+  if ( mode_changed ) autopilot_send_mode();
 
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
   /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
@@ -468,17 +466,14 @@ void navigation_task( void ) {
       if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
         last_pprz_mode = pprz_mode;
         pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
-        // FIXME
-        //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+        autopilot_send_mode();
         gps_lost = TRUE;
       }
     } else if (gps_lost) { /* GPS is ok */
       /** If aircraft was in failsafe mode, come back in previous mode */
       pprz_mode = last_pprz_mode;
       gps_lost = FALSE;
-
-      // FIXME
-      //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+      autopilot_send_mode();
     }
   }
 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
@@ -495,11 +490,9 @@ void navigation_task( void ) {
   CallTCAS();
 #endif
 
-//#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
-//  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
-//#endif
-
-  //SEND_CAM(DefaultChannel, DefaultDevice);
+#ifndef PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
+  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+#endif
 
   /* The nav task computes only nav_altitude. However, we are interested
      by desired_altitude (= nav_alt+alt_shift) in any case.
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
index 51da63b2e8..19cf19d53e 100644
--- a/sw/airborne/modules/cam_control/cam.c
+++ b/sw/airborne/modules/cam_control/cam.c
@@ -104,17 +104,20 @@ static void send_cam(void) {
   SEND_CAM(DefaultChannel, DefaultDevice);
 }
 
+#ifdef SHOW_CAM_COORDINATES
 static void send_cam_point(void) {
   DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice,
       &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
 }
-
+#endif
 
 void cam_init( void ) {
   cam_mode = CAM_MODE0;
 
   register_periodic_telemetry(DefaultPeriodic, "CAM", send_cam);
+#ifdef SHOW_CAM_COORDINATES
   register_periodic_telemetry(DefaultPeriodic, "CAM_POINT", send_cam_point);
+#endif
 }
 
 void cam_periodic( void ) {