From 810c06e8bca2e7bd52c2263f15d5d975d50f390e Mon Sep 17 00:00:00 2001
From: Anton Schirg <anton.schirg@posteo.de>
Date: Sun, 29 Oct 2017 18:50:23 +0100
Subject: [PATCH] Android GPS Proxy script

---
 README.md            | 11 ++++++
 android_gps_proxy.py | 80 ++++++++++++++++++++++++++++++++++++++++++++
 launch_with_gps.sh   |  6 ++++
 3 files changed, 97 insertions(+)
 create mode 100644 android_gps_proxy.py
 create mode 100755 launch_with_gps.sh

diff --git a/README.md b/README.md
index 9ce33ff..226a932 100644
--- a/README.md
+++ b/README.md
@@ -43,3 +43,14 @@ TOOD coming sooner or later
 ### Wiring-Diagram
 
 ![Wiring diagram](https://github.com/penguinmenac3/alice-hardware/blob/master/Wiring%20Alice.png?raw=true)
+
+## Android GPS and Compass
+You can use an Android smartphone as GPS and compass.
+
+Download this app: <https://play.google.com/store/apps/details?id=de.lorenz_fenster.sensorstreamgps> / <https://sourceforge.net/projects/smartphone-imu/>
+
+Enter your robots IP, set the stream type to UDP, select the GPS and Orientation sensor and enable "Include User-Checked Sensor Data in Stream".
+
+Mount the smartphone on the robot upwards with the back side facing in driving direction.
+
+Run the `android_gps_proxy.py` script on your robot or start with the `launch_with_gps.sh` shell script. The script will forward the UDP packets from the app to the `main.py` script.
diff --git a/android_gps_proxy.py b/android_gps_proxy.py
new file mode 100644
index 0000000..6fbd674
--- /dev/null
+++ b/android_gps_proxy.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+import socket
+from math import sqrt, pi
+import time
+
+# Receives GPS and Compass from the Sensorstream app via UDP and forwards it to alice via TCP
+# Download app from here https://play.google.com/store/apps/details?id=de.lorenz_fenster.sensorstreamgps
+
+last_gps = [-1.0, -1.0, -1.0]
+last_angle = -1
+
+def send(sock):
+    global last_gps, last_angle
+    sock.send("{} {} {} {} {} {}\n".format(last_gps[0], last_gps[1], last_gps[2], last_angle, -1, -1).encode("utf-8"))
+
+def main():
+    global last_gps, last_angle
+    UDP_IP = "0.0.0.0"
+    UDP_PORT = 5555
+
+    sock = socket.socket(socket.AF_INET, # Internet
+                         socket.SOCK_DGRAM) # UDP
+    sock.bind((UDP_IP, UDP_PORT))
+
+    OUTPUT_HOST = "localhost"
+    OUTPUT_PORT = 2323
+
+
+    while True:
+        try:
+            outs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            outs.connect((OUTPUT_HOST, OUTPUT_PORT))
+
+            while True:
+                data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
+                data_str = data.decode("utf-8")
+                #print(data_str)
+                splitted = data_str.split(',')
+                the_time = splitted[0]
+                splitted_no_time = splitted[1:]
+                #print('------')
+                #print(splitted_no_time)
+                i = 0
+                while i < len(splitted_no_time):
+                    num = int(splitted_no_time[i])
+                    if num == 1:
+                        data = list(map(float, splitted_no_time[i+1:i+4]))
+                        print("GPS:", data)
+                        last_gps = data
+                        send(outs)
+                    elif num == 8:
+                        i += 2  # Number 8 is has only one data field
+                        continue
+                    elif num == 81:
+                        data = list(map(float, splitted_no_time[i+1:i+4]))
+                        angle_to_north = data[0]
+                        if angle_to_north > 180.0:
+                            angle_to_north -= 360.0
+                        angle_to_north *= pi / 180.0
+                        print("Angle to north:", angle_to_north)
+                        last_angle = angle_to_north
+                        send(outs)
+                    '''
+                    elif num == 84:
+                        # https://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-rotate
+                        data = list(map(float, splitted_no_time[i+1:i+4]))
+                        xyz = data
+                        w = sqrt(1.0 - xyz[0] ** 2 - xyz[1] ** 2 - xyz[2] ** 2)  # Apparently wrong
+                        quat = xyz + [w]
+                        print(quat)
+                    '''
+                    i += 4
+        except Exception as e:
+            print("Error \"{}\", will try to reconnect".format(e))
+
+        outs.close()
+        time.sleep(2.0)
+
+if __name__ == "__main__":
+    main()
diff --git a/launch_with_gps.sh b/launch_with_gps.sh
new file mode 100755
index 0000000..2a92437
--- /dev/null
+++ b/launch_with_gps.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+cd /home/pi/git/alice-hardware
+
+screen -dmS alice-hardware python main.py
+screen -dmS alice-hardware-gps python android_gps_proxy.py