From 370113f580f2e6fbef3b3cca4d57eaf240315cd6 Mon Sep 17 00:00:00 2001
From: Ryosuke <rsasaki@keio.jp>
Date: Thu, 18 Jul 2019 09:36:28 +0900
Subject: [PATCH 1/2] =?UTF-8?q?=E3=83=9E=E3=83=AB=E3=83=81=E3=82=B9?=
 =?UTF-8?q?=E3=83=AC=E3=83=83=E3=83=89=E5=8C=96=E3=80=81=E3=81=BE=E3=81=A0?=
 =?UTF-8?q?=E9=80=94=E4=B8=AD=E3=81=A7=E3=81=99?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 raspberry_pi/gps_homing.py | 98 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 98 insertions(+)
 create mode 100644 raspberry_pi/gps_homing.py

diff --git a/raspberry_pi/gps_homing.py b/raspberry_pi/gps_homing.py
new file mode 100644
index 0000000..f72c68a
--- /dev/null
+++ b/raspberry_pi/gps_homing.py
@@ -0,0 +1,98 @@
+#!/usr/bin/python
+# -*- coding:utf-8 -*-
+
+from lib import rover_gps
+from lib import camera
+from lib import capture
+from lib import servo
+from lib import MPU6050
+from lib import pid
+import time
+import threading
+import queue
+
+Kp, Ki, Kd = 0.1, 0.1, 0.1
+goal_pos = [0, 0]
+position = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+pinL, pinR = 13, 18
+
+timeout = 5
+
+pos1 = [None, None]
+r_theta_to_goal = [None, None]
+azimuth_lock = threading.Lock()
+
+
+
+class update_azimuth_with_gyro(threading.Thread):
+    def __init__(self):
+        super(update_azimuth_with_gyro, self).__init__()
+        self.preT = time.time()
+        self.pre_gyro = mpu.get_gyro_data_lsb()[2]
+        self.azimuth = 0
+
+    def run(self):
+        while True:
+            global r_theta_to_goal, azimuth_lock
+            self.nowT = time.time()
+            self.gyro = mpu.get_gyro_data_lsb()[2]
+
+            azimuth_lock.acquire()
+            r_theta_to_goal[1] += (self.gyro + self.pre_gyro) * (self.nowT - self.preT) / 2
+            azimuth_lock.release()
+
+            self.pre_gyro, self.preT = self.gyro, self.nowT
+
+
+
+def update_azimuth_with_gps(pos1):
+    UAwG_thread = threading.Timer(5, update_azimuth_with_gps)
+    UAwG_thread.start()
+    t = time.time()
+    try:
+        global r_theta_to_goal, azimuth_lock
+        pos2 = [None, None]
+        while (pos2[0] is None) and (pos2[1] is None):
+            if (time.time() - t) > timeout:
+                raise TimeoutError
+            pos2 = gps.lat_long_measurement()            
+
+        azimuth_lock.acquire()
+        r_theta_to_goal = gps.convert_lat_long_to_r_theta(pos1[0], pos1[1], pos2[0], pos2[1])
+        azimuth_lock.release()
+
+        pos1 = pos2
+    except:
+        pass
+    finally:
+        pass
+
+
+
+
+
+try:
+    svL, svR = servo(pinL), servo(pinR)
+    pid = pid(0.1, 0.1, 0.1)
+    gps = rover_gps()
+    mpu = MPU6050(0x68)
+    '''
+    release detection
+    ↓
+    release parachute
+    ↓
+    landing detection
+    ↓
+    GPS/GYRO Navigation
+    ↓
+    TV Navigation
+    '''
+    while (pos1[0] is None) and (pos1[1] is None):
+        pos1 = gps.lat_long_measurement()
+
+
+except:
+    pass
+
+finally:
+    del svL, svR, pid

From 2005b8bba74094276c5f9c0bd1b4b1e4ee782b35 Mon Sep 17 00:00:00 2001
From: Ryosuke <rsasaki@keio.jp>
Date: Thu, 18 Jul 2019 10:07:21 +0900
Subject: [PATCH 2/2] fix

---
 raspberry_pi/gps_homing.py | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/raspberry_pi/gps_homing.py b/raspberry_pi/gps_homing.py
index f72c68a..8b31872 100644
--- a/raspberry_pi/gps_homing.py
+++ b/raspberry_pi/gps_homing.py
@@ -1,7 +1,7 @@
 #!/usr/bin/python
 # -*- coding:utf-8 -*-
 
-from lib import rover_gps
+from lib import rover_gps as gps
 from lib import camera
 from lib import capture
 from lib import servo
@@ -9,7 +9,6 @@
 from lib import pid
 import time
 import threading
-import queue
 
 Kp, Ki, Kd = 0.1, 0.1, 0.1
 goal_pos = [0, 0]
@@ -45,12 +44,12 @@ def run(self):
 
 
 
-def update_azimuth_with_gps(pos1):
+def update_azimuth_with_gps():
     UAwG_thread = threading.Timer(5, update_azimuth_with_gps)
     UAwG_thread.start()
     t = time.time()
     try:
-        global r_theta_to_goal, azimuth_lock
+        global r_theta_to_goal, pos1, azimuth_lock
         pos2 = [None, None]
         while (pos2[0] is None) and (pos2[1] is None):
             if (time.time() - t) > timeout:
@@ -62,8 +61,6 @@ def update_azimuth_with_gps(pos1):
         azimuth_lock.release()
 
         pos1 = pos2
-    except:
-        pass
     finally:
         pass
 
@@ -74,7 +71,6 @@ def update_azimuth_with_gps(pos1):
 try:
     svL, svR = servo(pinL), servo(pinR)
     pid = pid(0.1, 0.1, 0.1)
-    gps = rover_gps()
     mpu = MPU6050(0x68)
     '''
     release detection