diff --git a/sw/airborne/autopilot.c b/sw/airborne/autopilot.c
index e039c0bc3e..01ea443e1e 100644
--- a/sw/airborne/autopilot.c
+++ b/sw/airborne/autopilot.c
@@ -198,7 +198,12 @@ bool autopilot_set_mode(uint8_t new_autopilot_mode)
 #else
   autopilot_static_set_mode(new_autopilot_mode);
 #endif
-  return (autopilot.mode != mode);
+  if (autopilot.mode != mode) {
+    autopilot_send_mode();
+    return true;
+  } else {
+    return false;
+  }
 }
 
 /** AP mode setting handler
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.c b/sw/airborne/firmwares/fixedwing/autopilot_static.c
index e170eb9b96..78c64654a8 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_static.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot_static.c
@@ -215,18 +215,15 @@ void navigation_task(void)
  */
 void autopilot_failsafe_checks(void)
 {
-  uint8_t mode_changed = false;
-
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
   /* check normal mode from RC channel(s)
    */
   if (!RadioControlIsLost()) {
     bool pprz_mode_changed = pprz_mode_update();
-    mode_changed |= pprz_mode_changed;
 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
     bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values);
     rc_settings(calib_mode_changed || pprz_mode_changed);
-    mode_changed |= calib_mode_changed;
+    calib_mode_changed;
 #endif
   }
 
@@ -237,7 +234,7 @@ void autopilot_failsafe_checks(void)
   if (RadioControlIsLost() &&
       (autopilot_get_mode() == AP_MODE_AUTO1 ||
        autopilot_get_mode() == AP_MODE_MANUAL)) {
-    mode_changed |= autopilot_set_mode(RC_LOST_MODE);
+    autopilot_set_mode(RC_LOST_MODE);
   }
 
   /* Check RC kill switch
@@ -274,12 +271,12 @@ void autopilot_failsafe_checks(void)
       if (autopilot_get_mode() == AP_MODE_AUTO2 ||
           autopilot_get_mode() == AP_MODE_HOME) {
         last_pprz_mode = autopilot_get_mode();
-        mode_changed |= autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER);
+        autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER);
         gps_lost = true;
       }
     } else if (gps_lost) { /* GPS is ok */
       /** If aircraft was in failsafe mode, come back in previous mode */
-      mode_changed |= autopilot_set_mode(last_pprz_mode);
+      autopilot_set_mode(last_pprz_mode);
       gps_lost = false;
     }
   }
@@ -291,14 +288,9 @@ void autopilot_failsafe_checks(void)
       autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER &&
       autopilot.launch) {
     if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
-      mode_changed |= autopilot_set_mode(AP_MODE_HOME);
+      autopilot_set_mode(AP_MODE_HOME);
     }
   }
-
-  // send new mode if needed
-  if (mode_changed) {
-    autopilot_send_mode();
-  }
 }
 
 
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
index 93cf086fcf..be860d51ff 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
@@ -143,7 +143,7 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev)
   if(electrical.avg_cnt != 0) {
     avg_power = (float)electrical.avg_power / electrical.avg_cnt;
   }
-  
+
   pprz_msg_send_ENERGY(trans, dev, AC_ID,
                        &throttle, &electrical.vsupply, &electrical.current, &power, &avg_power, &electrical.charge, &electrical.energy);
 }
@@ -258,6 +258,13 @@ void autopilot_firmware_init(void)
 #endif
 }
 
+void autopilot_send_mode(void)
+{
+#if DOWNLINK
+  send_status(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
+#endif
+}
+
 /** autopilot event function
  *
  * used for automatic ground detection