From 7820d96f4c93b91ec334ec8c7cac20ec730f0d0b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 23 Dec 2011 14:22:44 -0800 Subject: [PATCH] No longer blocking before initing home --- ArduCopter/commands.pde | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 1f2baa1207..954cfa7c62 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -192,20 +192,9 @@ static void set_next_WP(struct Location *wp) static void init_home() { home_is_set = true; - - // block until we get a good fix - // ----------------------------- - while (!g_gps->new_data || !g_gps->fix) { - g_gps->update(); - // we need GCS update while waiting for GPS, to ensure - // we react to HIL mavlink - gcs_update(); - } - home.id = MAV_CMD_NAV_WAYPOINT; home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 - //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid home.alt = 0; // Home is always 0 // to point yaw towards home until we set it with Mavlink @@ -220,7 +209,7 @@ static void init_home() // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; - // + // in case we RTL next_WP = home; // Load home for a default guided_WP