diff --git a/res/layout/fragment_launcher.xml b/res/layout/fragment_launcher.xml
index 6a38ac8..bc3f30b 100644
--- a/res/layout/fragment_launcher.xml
+++ b/res/layout/fragment_launcher.xml
@@ -77,4 +77,16 @@
             android:singleLine="true"
             android:text="@string/launcher_swipe_right_text_content" />
     </RelativeLayout>
+
+    <TextView
+        android:id="@+id/launcher_failsafe_text"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:textStyle="bold"
+        android:textSize="24sp"
+        android:textColor="#F00"
+        android:visibility="invisible"
+        android:text="@string/launcher_failsafe_text_content" />
 </RelativeLayout>
diff --git a/res/values/strings_launcher_fragment.xml b/res/values/strings_launcher_fragment.xml
index 02e80f1..2003eed 100644
--- a/res/values/strings_launcher_fragment.xml
+++ b/res/values/strings_launcher_fragment.xml
@@ -7,6 +7,7 @@
     <string name="launcher_home_text_content">[No Home Position]</string>
     <string name="launcher_ip_text_content">[IP Address]</string>
     <string name="launcher_home_text_format">[HOME] Lat: <b>%1$f</b>, Lon: <b>%2$f</b></string>
+    <string name="launcher_failsafe_text_content">Failsafe Engaged</string>
     <string name="launcher_swipe_left_text_content">Swipe left for settings.</string>
     <string name="launcher_swipe_right_text_content">Swipe right for debugging.</string>
 </resources>
diff --git a/res/values/strings_preferences.xml b/res/values/strings_preferences.xml
index c231355..486cf97 100644
--- a/res/values/strings_preferences.xml
+++ b/res/values/strings_preferences.xml
@@ -17,6 +17,7 @@
     <string name="pref_failsafe_timeout_summary">Set the maximum time without connectivity until failsafe activates.</string>
     <string name="pref_failsafe_timeout_default">120</string>
     <string-array name="pref_failsafe_timeout_entries">
+        <item>15 seconds</item>
         <item>30 seconds</item>
         <item>1 minute</item>
         <item>2 minutes</item>
@@ -24,11 +25,12 @@
         <item>10 minutes</item>
     </string-array>
     <string-array name="pref_failsafe_timeout_values">
-        <item>30</item>
-        <item>60</item>
-        <item>120</item>
-        <item>300</item>
-        <item>600</item>
+        <item>15000</item>
+        <item>30000</item>
+        <item>60000</item>
+        <item>120000</item>
+        <item>300000</item>
+        <item>600000</item>
     </string-array>
     <string name="pref_vehicle_category_title">Vehicle Configuration</string>
     <string name="pref_vehicle_type_title">Vehicle Type</string>
diff --git a/res/xml/preferences.xml b/res/xml/preferences.xml
index 99a5f9c..dc2a6d4 100644
--- a/res/xml/preferences.xml
+++ b/res/xml/preferences.xml
@@ -43,6 +43,7 @@
             android:title="@string/pref_failsafe_addr_title" />
         <ListPreference
             android:defaultValue="@string/pref_failsafe_timeout_default"
+            android:dependency="pref_failsafe_enable"
             android:dialogTitle="@string/pref_failsafe_timeout_title"
             android:entries="@array/pref_failsafe_timeout_entries"
             android:entryValues="@array/pref_failsafe_timeout_values"
diff --git a/src/com/platypus/android/server/Failsafe.java b/src/com/platypus/android/server/Failsafe.java
new file mode 100644
index 0000000..bfc7eb5
--- /dev/null
+++ b/src/com/platypus/android/server/Failsafe.java
@@ -0,0 +1,395 @@
+package com.platypus.android.server;
+
+import android.content.Context;
+import android.content.Intent;
+import android.content.SharedPreferences;
+import android.preference.PreferenceManager;
+import android.util.Log;
+
+import com.platypus.crw.VehicleServer;
+import com.platypus.crw.data.UtmPose;
+
+import java.io.IOException;
+import java.net.InetAddress;
+import java.net.UnknownHostException;
+import java.util.concurrent.CancellationException;
+import java.util.concurrent.ExecutionException;
+import java.util.concurrent.ScheduledFuture;
+import java.util.concurrent.ScheduledThreadPoolExecutor;
+import java.util.concurrent.TimeUnit;
+
+/**
+ * Implements a failsafe behavior that does the following:
+ *
+ * - Every five seconds, query the failsafe address for connectivity.
+ * - Record the current pose if the boat is within connectivity.
+ *
+ * - If it has been FAILSAFE_TIMEOUT since the failsafe address was contacted,
+ *   then enable autonomy and head to the last position where there was connectivity.
+ *
+ * - If, after reaching the last-known position, it has still been longer than FAILSAFE_TIMEOUT
+ *   since the failsafe address was contacted, then enable autonomy to head to the home position,
+ *   if it was specified.
+ */
+public class Failsafe {
+    private static final String TAG = VehicleService.class.getSimpleName();
+    static final String ACTION_FAILSAFE_UPDATE = "com.platypus.android.server.action.FAILSAFE_UPDATE";
+
+    /**
+     * Time between periodic connectivity checks.
+     */
+    static int UPDATE_PERIOD_MS = 5000;
+
+    /**
+     * The current status of the failsafe behavior.
+     */
+    enum FailsafeStatus {
+        CONNECTED,
+        FAILSAFE_TO_LAST_LOCATION,
+        FAILSAFE_TO_HOME_LOCATION
+    }
+
+    /**
+     * Failsafe server to which to test connectivity.
+     * Updated from SharedPreferences.
+     */
+    InetAddress mFailsafeServer;
+
+    /**
+     * Maximum allowable loss of connectivity to failsafe server before engaging failsafe.
+     * Updated from SharedPreferences.
+     */
+    long mTimeoutMs = 0;
+
+    /**
+     * Pose at which the vehicle was last connected to the failsafe server.
+     */
+    UtmPose mLastConnectedLocation;
+
+    /**
+     * Home location to which the vehicle will navigate if last-known location still fails.
+     */
+    UtmPose mHomeLocation;
+
+    /**
+     * Time at which the vehicle was last connected to the failsafe server.
+     */
+    long mLastConnectedTime;
+
+    /**
+     * Current state of the failsafe recovery behavior.
+     */
+    FailsafeStatus mStatus = FailsafeStatus.CONNECTED;
+
+    /**
+     * Vehicle on which to engage the failsafe behavior.
+     */
+    final VehicleServer mVehicleServer;
+
+    /**
+     * Application context within which this behavior is being run.
+     */
+    final Context mContext;
+
+    /**
+     * Lock used to synchronize changes in failsafe server settings with updates.
+     */
+    final Object mUpdateLock = new Object();
+
+    /**
+     * Future for a running update task.
+     */
+    ScheduledFuture mUpdateFuture;
+
+    /**
+     * Executor that periodically runs connectivity checks.
+     */
+    final ScheduledThreadPoolExecutor mUpdateExecutor = new ScheduledThreadPoolExecutor(1);
+
+    public Failsafe(final Context context, final VehicleServer server) {
+        mContext = context;
+        mVehicleServer = server;
+
+        // Register a shared preference listener to listen for settings updates and
+        // initialize based on existing shared preferences settings.
+        SharedPreferences sharedPreferences =
+            PreferenceManager.getDefaultSharedPreferences(mContext);
+        sharedPreferences.registerOnSharedPreferenceChangeListener(mPreferenceListener);
+        updatePreferences(sharedPreferences);
+    }
+
+    /**
+     * Permanently shuts down the failsafe behavior.
+     * After this function is called, this instance is no longer valid.
+     */
+    public void shutdown() {
+        // Unregister the shared preference listener.
+        PreferenceManager.getDefaultSharedPreferences(mContext)
+                .unregisterOnSharedPreferenceChangeListener(mPreferenceListener);
+
+        // Shutdown the update task if it is running.
+        synchronized (mUpdateLock) {
+            if (mUpdateFuture != null) {
+                try {
+                    mUpdateFuture.cancel(true);
+                    mUpdateFuture.get();
+                } catch (InterruptedException | CancellationException | ExecutionException e) {
+                    // Do nothing.
+                }
+                mUpdateFuture = null;
+            }
+        }
+
+        // Broadcast that the failsafe is no longer running.
+        sendFailsafeIntent(false);
+    }
+
+    /**
+     * Run an update loop which tests connectivity and takes appropriate failsafe measures.
+     */
+    Runnable mUpdateRunnable = new Runnable() {
+        @Override
+        public void run() {
+            // Run a connectivity test.
+            checkConnection();
+
+            // Test if connectivity is sufficiently recent.
+            boolean isRecentlyConnected;
+            synchronized (mUpdateLock) {
+                isRecentlyConnected = (System.currentTimeMillis() - mLastConnectedTime < mTimeoutMs);
+            }
+            sendFailsafeIntent(!isRecentlyConnected);
+
+            switch (mStatus) {
+                case CONNECTED:
+                    // If we were recently connected, just stay connected.
+                    if (isRecentlyConnected)
+                        return;
+
+                    // If not recently connected, initiate failsafe to last known location.
+                    synchronized (mUpdateLock) {
+                        mStatus = FailsafeStatus.FAILSAFE_TO_LAST_LOCATION;
+                        Log.w(TAG,
+                                "Connectivity loss detected. " +
+                                "Will failsafe to last-known location if IDLE.");
+                    }
+                    break;
+
+                case FAILSAFE_TO_LAST_LOCATION:
+                    // If we were recently connected, return to connected state.
+                    // (Note that this will NOT stop the ongoing navigation command.  That is left up
+                    //  to the user to override when connectivity is re-established.)
+                    if (isRecentlyConnected) {
+                        synchronized (mUpdateLock) {
+                            mStatus = FailsafeStatus.CONNECTED;
+                        }
+                        Log.i(TAG,
+                                "Failsafe to last-known location disengaged, " +
+                                "connectivity re-established.");
+                        return;
+                    }
+
+                    // Check if we are already navigating to the last-known location.
+                    // If not, start navigating using the default controller.
+                    synchronized (mUpdateLock) {
+                        if (mLastConnectedLocation != null)
+                            checkFailsafeBehavior(mLastConnectedLocation);
+
+                        // Since we have already attempted to get to the last location,
+                        // transition to using the home location the next time we need to
+                        // engage a failsafe behavior.
+                        mStatus = FailsafeStatus.FAILSAFE_TO_HOME_LOCATION;
+                    }
+                    break;
+
+                case FAILSAFE_TO_HOME_LOCATION:
+                    // If we were recently connected, return to connected state.
+                    // (Note that this will NOT stop the ongoing navigation command.  That is left up
+                    //  to the user to override when connectivity is re-established.)
+                    if (isRecentlyConnected) {
+                        synchronized (mUpdateLock) {
+                            mStatus = FailsafeStatus.CONNECTED;
+                        }
+
+                        Log.i(TAG,
+                                "Failsafe to home location disengaged, " +
+                                "connectivity established.");
+                        return;
+                    }
+
+                    // Check if we are already navigating to the home location.
+                    // If not, start navigating using the default controller.
+                    synchronized (mUpdateLock) {
+                        if (mHomeLocation != null)
+                            checkFailsafeBehavior(mHomeLocation);
+                    }
+                    break;
+
+                default:
+                    Log.e(TAG, "Unexpectedly reached invalid status, resetting to CONNECTED.");
+                    mStatus = FailsafeStatus.CONNECTED;
+                    break;
+            }
+        }
+    };
+
+    /**
+     * Executes a single ICMP ping to a host and returns the success.
+     *
+     * @param host the host to attempt to reach
+     * @return true if the ping succeeded.
+     */
+    boolean ping(InetAddress host) {
+        Runtime runtime = Runtime.getRuntime();
+        try {
+            Process pingProcess = runtime.exec(
+                    "/system/bin/ping -c 1 -W 1 " + host.getHostAddress());
+            int result = pingProcess.waitFor();
+            return (result == 0);
+        } catch (IOException | InterruptedException e) {
+            return false;
+        }
+    }
+
+    /**
+     * Tests connection to the failsafe server and updates the last connected location and time.
+     *
+     * @return the success of the connectivity test
+     */
+    boolean checkConnection() {
+        // Get the current failsafe server address.
+        InetAddress failsafeServer;
+        synchronized (mUpdateLock) {
+            failsafeServer = mFailsafeServer;
+        }
+
+        // If the server is not reachable, do nothing.
+        if (!ping(failsafeServer))
+            return false;
+
+        // If the server is reached, update the latest pose and time.
+        synchronized (mUpdateLock) {
+            mLastConnectedTime = System.currentTimeMillis();
+            mLastConnectedLocation = mVehicleServer.getPose();
+            return true;
+        }
+    }
+
+    /**
+     * Sends an android Intent that notifies listeners that the state of the failsafe has changed.
+     *
+     * @param isRunning whether the failsafe is currently engaged or not.
+     */
+    void sendFailsafeIntent(boolean isRunning) {
+        Intent intent = new Intent(ACTION_FAILSAFE_UPDATE);
+        intent.putExtra("isRunning", isRunning);
+        mContext.sendBroadcast(intent);
+    }
+
+    /**
+     * Check if we are already navigating. If not, start navigating using the default controller.
+     *
+     * This will begin navigation to the failsafe, but only if the boat does not have any active
+     * autonomous behavior.
+     *
+     * @param failsafeDestination the failsafe destination to verify navigation towards
+     */
+    void checkFailsafeBehavior(final UtmPose failsafeDestination) {
+
+        // Ignore attempts to set a null destination.
+        if (failsafeDestination == null) {
+            Log.w(TAG, "Ignoring attempt to set null destination.");
+            return;
+        }
+
+        // Do not engage failsafe if the vehicle is already doing something.
+        switch(mVehicleServer.getWaypointStatus()) {
+            case GOING:
+            case PAUSED:
+                return;
+        }
+
+        // Check if already navigating to the desired destination.
+        UtmPose[] waypoints = mVehicleServer.getWaypoints();
+        if (waypoints.length > 0 && waypoints[0].equals(failsafeDestination))
+            return;
+
+        // Start navigating to the failsafe destination.
+        mVehicleServer.startWaypoints(new UtmPose[]{failsafeDestination}, null);
+        mVehicleServer.setAutonomous(true);
+        Log.w(TAG, "Engaged failsafe behavior to: " + failsafeDestination);
+    }
+
+    /**
+     * Updates all member variables from shared preference settings.
+     */
+    void updatePreferences(final SharedPreferences sharedPreferences) {
+        synchronized (mUpdateLock) {
+
+            // Update the timeout used for the failsafe server (stored as a numeric string).
+            mTimeoutMs = Long.parseLong(
+                    sharedPreferences.getString("pref_failsafe_timeout",
+                            mContext.getString(R.string.pref_failsafe_timeout_default)));
+
+            // Update the hostname used for the failsafe server.
+            String failsafeHostname =
+                    sharedPreferences.getString("pref_failsafe_addr", "localhost");
+            try {
+                mFailsafeServer = InetAddress.getByName(failsafeHostname);
+            } catch (UnknownHostException e) {
+                Log.w(TAG, "Unable to resolve failsafe server: " +
+                        failsafeHostname);
+            }
+
+            // Enable/disable update task.
+            if (sharedPreferences.getBoolean("pref_failsafe_enable", false)) {
+                if (mUpdateFuture == null) {
+                    // Reset last-known location as for failsafe.
+                    mLastConnectedLocation = null;
+                    mLastConnectedTime = System.currentTimeMillis();
+                    mStatus = FailsafeStatus.CONNECTED;
+
+                    // Start update task.
+                    mUpdateFuture = mUpdateExecutor.scheduleAtFixedRate(
+                            mUpdateRunnable, 0,
+                            UPDATE_PERIOD_MS, TimeUnit.MILLISECONDS);
+                    Log.i(TAG, "Failsafe service enabled.");
+                }
+            } else {
+                if (mUpdateFuture != null) {
+                    // Stop update task.
+                    try {
+                        mUpdateFuture.cancel(true);
+                        mUpdateFuture.get();
+                    } catch (InterruptedException | CancellationException | ExecutionException e) {
+                        // Do nothing.
+                    }
+                    mUpdateFuture = null;
+                    Log.i(TAG, "Failsafe service disabled.");
+
+                    // Broadcast that the failsafe behavior is no longer running.
+                    sendFailsafeIntent(false);
+                }
+            }
+        }
+    }
+
+    /**
+     * A shared preference listener that changes settings on the failsafe server when
+     * settings are changed.
+     */
+    private SharedPreferences.OnSharedPreferenceChangeListener mPreferenceListener =
+            new SharedPreferences.OnSharedPreferenceChangeListener() {
+                @Override
+                public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
+                    // If any of the keys related to this service changed, update them.
+                    switch (key) {
+                        case "pref_failsafe_enable":
+                        case "pref_failsafe_addr":
+                        case "pref_failsafe_timeout":
+                            updatePreferences(sharedPreferences);
+                            break;
+                    }
+                }
+            };
+}
diff --git a/src/com/platypus/android/server/LauncherFragment.java b/src/com/platypus/android/server/LauncherFragment.java
index 22199fd..629443e 100644
--- a/src/com/platypus/android/server/LauncherFragment.java
+++ b/src/com/platypus/android/server/LauncherFragment.java
@@ -2,8 +2,10 @@
 
 import android.app.ActivityManager;
 import android.app.Fragment;
+import android.content.BroadcastReceiver;
 import android.content.Context;
 import android.content.Intent;
+import android.content.IntentFilter;
 import android.content.SharedPreferences;
 import android.location.Location;
 import android.location.LocationListener;
@@ -41,11 +43,12 @@ public class LauncherFragment extends Fragment
 
     final Handler mHandler = new Handler();
 
-    protected TextView mHomeText;
-    protected TextView mIpAddressText;
-    protected Button mLaunchButton;
-    protected Button mSetHomeButton;
-    protected LocationManager mLocationManager;
+    TextView mHomeText;
+    TextView mFailsafeText;
+    TextView mIpAddressText;
+    Button mLaunchButton;
+    Button mSetHomeButton;
+    LocationManager mLocationManager;
 
     @Override
     public void onCreate(Bundle savedInstanceState) {
@@ -62,6 +65,7 @@ public View onCreateView(LayoutInflater inflater, ViewGroup container,
 
         // Get references to UI elements.
         mHomeText = (TextView) view.findViewById(R.id.launcher_home_text);
+        mFailsafeText = (TextView) view.findViewById(R.id.launcher_failsafe_text);
         mIpAddressText = (TextView) view.findViewById(R.id.ip_address_text);
         mLaunchButton = (Button) view.findViewById(R.id.launcher_launch_button);
         mSetHomeButton = (Button) view.findViewById(R.id.launcher_home_button);
@@ -72,9 +76,22 @@ public View onCreateView(LayoutInflater inflater, ViewGroup container,
         // Add listener for home button click.
         mSetHomeButton.setOnLongClickListener(new SetHomeListener());
 
+        // Add listener for failsafe behavior events.
+        getActivity().registerReceiver(
+                mFailsafeBroadcastReceiver,
+                new IntentFilter(Failsafe.ACTION_FAILSAFE_UPDATE));
+
         return view;
     }
 
+    @Override
+    public void onDestroy() {
+        super.onDestroy();
+
+        // Remove listener for failsafe behavior events.
+        getActivity().unregisterReceiver(mFailsafeBroadcastReceiver);
+    }
+
     @Override
     public void onResume() {
         super.onResume();
@@ -113,6 +130,22 @@ protected void updateHomeLocation() {
         }
     }
 
+    /**
+     * Receives failsafe broadcast intents and updates the launcher UI accordingly.
+     */
+    final BroadcastReceiver mFailsafeBroadcastReceiver = new BroadcastReceiver() {
+        @Override
+        public void onReceive(final Context context, final Intent intent) {
+            LauncherFragment.this.getActivity().runOnUiThread(new Runnable() {
+                @Override
+                public void run() {
+                    boolean isRunning = intent.getBooleanExtra("isRunning", false);
+                    mFailsafeText.setVisibility(isRunning ? View.VISIBLE : View.INVISIBLE);
+                }
+            });
+        }
+    };
+
     /**
      * Updates the launch button depending on whether the service is running or not.
      */
@@ -126,6 +159,7 @@ protected void updateLaunchStatus() {
             mLaunchButton.setBackground(getResources().getDrawable(
                     R.drawable.fragment_launcher_launch_button_background_red));
             mLaunchButton.setText(getResources().getString(R.string.launcher_launch_button_start));
+            mFailsafeText.setVisibility(View.INVISIBLE);
         }
     }
 
diff --git a/src/com/platypus/android/server/VehicleServerImpl.java b/src/com/platypus/android/server/VehicleServerImpl.java
index d8fc022..f9a354f 100644
--- a/src/com/platypus/android/server/VehicleServerImpl.java
+++ b/src/com/platypus/android/server/VehicleServerImpl.java
@@ -657,87 +657,78 @@ public void setPose(UtmPose pose) {
     }
 
     @Override
-    public void startWaypoints(final UtmPose[] waypoints,
-                               final String controller) {
-        Log.i(TAG, "Starting waypoints with " + controller + ": "
-                + Arrays.toString(waypoints));
-        if (controller.equalsIgnoreCase("PRIMITIVES")) {
-            _waypoints = new UtmPose[waypoints.length];
-            System.arraycopy(waypoints, 0, _waypoints, 0, _waypoints.length);
-            VehicleController vc = AirboatController.valueOf(controller).controller;
-            vc.update(VehicleServerImpl.this, (double) UPDATE_INTERVAL_MS / 1000.0);
-            Log.i(TAG, "Waypoint Status: PRIMITIVES");
-        } else {
-            // Create a waypoint navigation task
-            TimerTask newNavigationTask = new TimerTask() {
-                final double dt = (double) UPDATE_INTERVAL_MS / 1000.0;
-
-                // Retrieve the appropriate controller in initializer
-                VehicleController vc = DEFAULT_CONTROLLER;
-
-                {
-                    try {
-                        vc = (controller == null) ? vc : AirboatController.valueOf(controller).controller;
-                        //Log.i(TAG, "vc:"+ vc.toString() + "controller" + controller );
-                    } catch (IllegalArgumentException e) {
-                        Log.w(TAG, "Unknown controller specified (using " + vc
-                                + " instead): " + controller);
-                    }
-                }
+    public void startWaypoints(final UtmPose[] waypoints, final String controller) {
 
-                @Override
-                public void run() {
-                    synchronized (_navigationLock) {
-                        //Log.i(TAG, "Synchronized");
-
-                        if (!_isAutonomous.get()) {
-                            // If we are not autonomous, do nothing
-                            Log.i(TAG, "Paused");
-                            sendWaypointUpdate(WaypointState.PAUSED);
-                        } else if (_waypoints.length == 0) {
-                            // If we are finished with waypoints, stop in place
-                            Log.i(TAG, "Done");
-                            sendWaypointUpdate(WaypointState.DONE);
-                            setVelocity(new Twist(DEFAULT_TWIST));
-                            this.cancel();
-                            _navigationTask = null;
-
-                        } else {
-                            // If we are still executing waypoints, use a
-                            // controller to figure out how to get to waypoint
-                            // TODO: measure dt directly instead of approximating
-                            Log.i(TAG, "controller :" + controller);
-                            vc.update(VehicleServerImpl.this, dt);
-                            sendWaypointUpdate(WaypointState.GOING);
-                            //Log.i(TAG, "Waypoint Status: POINT_AND_SHOOT");
-                        }
+        // Determine the appropriate controller to use from the name.
+        VehicleController parsedVehicleController = DEFAULT_CONTROLLER;
+        try {
+            parsedVehicleController = (controller == null) ?
+                    DEFAULT_CONTROLLER : AirboatController.valueOf(controller).controller;
+        } catch (IllegalArgumentException e) {
+            Log.w(TAG, "Unknown controller specified (using " + parsedVehicleController
+                    + " instead): " + controller);
+        }
+        Log.i(TAG, "Starting waypoints with " + parsedVehicleController + ": " +
+                Arrays.toString(waypoints));
+
+        // Create a waypoint navigation task
+        final VehicleController vc = parsedVehicleController;
+        TimerTask newNavigationTask = new TimerTask() {
+            long lastUpdateTime = System.currentTimeMillis();
+
+            @Override
+            public void run() {
+                synchronized (_navigationLock) {
+                    if (!_isAutonomous.get()) {
+                        // If we are not autonomous, do nothing
+                        Log.i(TAG, "Paused");
+                        sendWaypointUpdate(WaypointState.PAUSED);
+
+                    } else if (_waypoints.length == 0) {
+                        // If we are finished with waypoints, stop in place
+                        Log.i(TAG, "Done");
+                        sendWaypointUpdate(WaypointState.DONE);
+                        setVelocity(new Twist(DEFAULT_TWIST));
+                        this.cancel();
+                        _navigationTask = null;
+
+                    } else {
+                        // Compute the time since the last update.
+                        long currentTime = System.currentTimeMillis();
+                        double dt = (currentTime - lastUpdateTime) / 1000.0;
+                        lastUpdateTime = currentTime;
+
+                        // If we are still executing waypoints, use a
+                        // controller to figure out how to complete the waypoint.
+                        vc.update(VehicleServerImpl.this, dt);
+                        sendWaypointUpdate(WaypointState.GOING);
                     }
                 }
-            };
+            }
+        };
 
-            synchronized (_navigationLock) {
-                // Change waypoints to new set of waypoints
-                _waypoints = new UtmPose[waypoints.length];
-                System.arraycopy(waypoints, 0, _waypoints, 0, _waypoints.length);
+        synchronized (_navigationLock) {
+            // Change waypoints to new set of waypoints
+            _waypoints = new UtmPose[waypoints.length];
+            System.arraycopy(waypoints, 0, _waypoints, 0, _waypoints.length);
 
-                // Cancel any previous navigation tasks
-                if (_navigationTask != null)
-                    _navigationTask.cancel();
+            // Cancel any previous navigation tasks
+            if (_navigationTask != null)
+                _navigationTask.cancel();
 
-                // Schedule this task for execution
-                _navigationTask = newNavigationTask;
-                _navigationTimer.scheduleAtFixedRate(_navigationTask, 0, UPDATE_INTERVAL_MS);
-            }
+            // Schedule this task for execution
+            _navigationTask = newNavigationTask;
+            _navigationTimer.scheduleAtFixedRate(_navigationTask, 0, UPDATE_INTERVAL_MS);
+        }
 
-            // Report the new waypoint in the log file.
-            try {
-                mLogger.info(new JSONObject()
-                        .put("nav", new JSONObject()
-                                .put("controller", controller)
-                                .put("waypoints", new JSONArray(waypoints))));
-            } catch (JSONException e) {
-                Log.w(TAG, "Unable to serialize waypoints.");
-            }
+        // Report the new waypoint in the log file.
+        try {
+            mLogger.info(new JSONObject()
+                    .put("nav", new JSONObject()
+                            .put("controller", controller)
+                            .put("waypoints", new JSONArray(waypoints))));
+        } catch (JSONException e) {
+            Log.w(TAG, "Unable to serialize waypoints.");
         }
     }
 
diff --git a/src/com/platypus/android/server/VehicleService.java b/src/com/platypus/android/server/VehicleService.java
index 6be5821..80c8cb2 100644
--- a/src/com/platypus/android/server/VehicleService.java
+++ b/src/com/platypus/android/server/VehicleService.java
@@ -68,9 +68,12 @@ public class VehicleService extends Service {
     // Reference to vehicle logfile.
     private VehicleLogger mLogger;
 
-    // Reference to vehicle controller;
+    // Reference to vehicle controller.
     private Controller mController;
 
+    // Reference to vehicle failsafe behavior.
+    private Failsafe mFailsafe;
+
     // Objects implementing actual functionality
     private VehicleServerImpl _vehicleServerImpl;
     private UdpVehicleService _udpService;
@@ -285,6 +288,9 @@ public int onStartCommand(final Intent intent, int flags, int startId) {
         // Create the internal vehicle server implementation.
         _vehicleServerImpl = new VehicleServerImpl(this, mLogger, mController);
 
+        // Initialize failsafe service.
+        mFailsafe = new Failsafe(this, _vehicleServerImpl);
+
         // Start up UDP vehicle service in the background
         new Thread(new Runnable() {
             @Override
@@ -399,6 +405,12 @@ public void onDestroy() {
             mController = null;
         }
 
+        // Shutdown the failsafe service.
+        if (mFailsafe != null) {
+            mFailsafe.shutdown();
+            mFailsafe = null;
+        }
+
         // Unregister shared preference listener to listen for updates.
         PreferenceManager.getDefaultSharedPreferences(this)
                 .unregisterOnSharedPreferenceChangeListener(mPreferenceListener);