Sub: Require initial contact with GCS before entering failsafe.

ArduSub/Sub.cpp

@@ -103,6 +103,8 @@ Sub::Sub(void) :
     // init sensor error logging flags
     sensor_health.baro = true;
     sensor_health.compass = true;
+
+    failsafe.last_heartbeat_ms = 0;
 }
 
 Sub sub;

ArduSub/events.cpp

@@ -126,9 +126,9 @@ void Sub::set_leak_status(bool status) {
 // failsafe_gcs_check - check for ground station failsafe
 void Sub::failsafe_gcs_check()
 {
-    // return immediately if gcs failsafe action is disabled
+    // return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled
     // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
-    if (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED) {
+    if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) {
     	return;
     }