|
|
|
@ -192,20 +192,9 @@ static void set_next_WP(struct Location *wp)
@@ -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()
@@ -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 |
|
|
|
|