From 70463dc572e3a777f6934f6b9ac97e0fefa01bb4 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 7 May 2016 16:26:49 +0900
Subject: [PATCH] Copter: auto_spline_start handles triggers terrain failsafe

Also immediately exit auto_wp_start on terrain failsafe
---
 ArduCopter/control_auto.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp
index 0ad251519f..b530f77576 100644
--- a/ArduCopter/control_auto.cpp
+++ b/ArduCopter/control_auto.cpp
@@ -206,6 +206,7 @@ void Copter::auto_wp_start(const Location_Class& dest_loc)
     if (!wp_nav.set_wp_destination(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         failsafe_terrain_on_event();
+        return;
     }
 
     // initialise yaw
@@ -275,9 +276,9 @@ void Copter::auto_spline_start(const Location_Class& destination, bool stopped_a
 
     // initialise wpnav
     if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
-        // failure to set destination (likely because of missing terrain data)
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
-        // To-Do: handle failure
+        // failure to set destination can only be because of missing terrain data
+        failsafe_terrain_on_event();
+        return;
     }
 
     // initialise yaw