From 277842c3effb03f19e2eb02f1e322c9b87500cd4 Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Mon, 12 Nov 2018 18:04:56 -0500
Subject: [PATCH] fix gohome manual

---
 buzz_scripts/include/act/naviguation.bzz |  4 ++--
 buzz_scripts/include/act/states.bzz      | 16 +++++++++-------
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz
index 5aec163..1425145 100644
--- a/buzz_scripts/include/act/naviguation.bzz
+++ b/buzz_scripts/include/act/naviguation.bzz
@@ -2,7 +2,7 @@ GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
-GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.lat=45.510400, .lng=-73.610421},
+GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
 			.2={.lat=45.510896, .lng=-73.608731},
 			.3={.lat=45.510355, .lng=-73.608404},
 			.4={.lat=45.509840, .lng=-73.610072}}
@@ -18,7 +18,7 @@ function goto_gps(transf) {
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)
 		gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
-		#geofence(gf)
+		geofence(gf)
 		#m_navigation = LCA(m_navigation)
 		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 	}
diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index a0b1903..e9b9988 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -74,13 +74,15 @@ gohomeT=100
 function goinghome() {
   BVMSTATE = "GOHOME"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
-		storegoal(homegps.lat, homegps.lng, pose.position.altitude)
-		goto_gps(AUTO_LAUNCH_STATE)
-		#if(gohomeT > 0) {	# TODO: Make a real check if home is reached
-		#	gohome()
-		#	gohomeT = gohomeT - 1
-		#} else
-		#	BVMSTATE = AUTO_LAUNCH_STATE
+		m_navigation = vec_from_gps(homegps.latitude, homegps.longitude, 0)
+		#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
+		if(math.vec2.length(m_navigation) < GOTODIST_TOL){ # reached destination
+				BVMSTATE = AUTO_LAUNCH_STATE
+		} else {
+			m_navigation = LimitSpeed(m_navigation, 3.0)
+			#m_navigation = LCA(m_navigation)
+			goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
+		}
 	}
 }