diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index c7be083f93..49f4736fe7 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -649,6 +649,8 @@ void loop()
 // update AHRS system
 static void ahrs_update()
 {
+    ahrs.set_armed(hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
+
 #if HIL_MODE != HIL_MODE_DISABLED
     // update hil before AHRS update
     gcs_update();
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 35cc254db2..6aa9f6d17b 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -78,7 +78,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
 #endif
 
     // we are armed if we are not initialising
-    if (control_mode != INITIALISING) {
+    if (control_mode != INITIALISING && ahrs.get_armed()) {
         base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index bbe8186388..723f8d8b7d 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -516,10 +516,7 @@ static bool should_log(uint32_t mask)
     if (!(mask & g.log_bitmask) || in_mavlink_delay) {
         return false;
     }
-    bool armed;
-    armed = (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
-
-    bool ret = armed || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0;
+    bool ret = ahrs.get_armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0;
     if (ret && !DataFlash.logging_started() && !in_log_download) {
         // we have to set in_mavlink_delay to prevent logging while
         // writing headers