diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 89c2ad1c29..0c741804b5 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -818,8 +818,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
 
     case MSG_OPTICAL_FLOW:
 #if OPTFLOW == ENABLED
-        CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
-        send_opticalflow(plane.ahrs, plane.optflow);
+        if (plane.optflow.enabled()) {        
+            CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
+            send_opticalflow(plane.ahrs, plane.optflow);
+        }
 #endif
         break;
 
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 7fc688eeba..784e638af1 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -179,7 +179,9 @@ void Plane::init_ardupilot()
     
 #if OPTFLOW == ENABLED
     // make optflow available to libraries
-    ahrs.set_optflow(&optflow);
+    if (optflow.enabled()) {
+        ahrs.set_optflow(&optflow);
+    }
 #endif
 
     // Register mavlink_delay_cb, which will run anytime you have
@@ -246,11 +248,9 @@ void Plane::init_ardupilot()
 
     // initialise sensor
 #if OPTFLOW == ENABLED
-    if (!optflow.enabled()) {
-        return;
+    if (optflow.enabled()) {
+        optflow.init();
     }
-
-    optflow.init();
 #endif
 
 }