-
Notifications
You must be signed in to change notification settings - Fork 1
/
serverV2.py
146 lines (123 loc) · 4.67 KB
/
serverV2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
#! python
#Created for Mission Critial Drone Project for Ericsson
#Code to be run on Pi on Drone
#Austin Greisman
import socket
import Adafruit_PCA9685
import time
import re
import sys
import select
#For integrating Accelerometer and Gyroscope: https://github.com/Tijndagamer/mpu6050
from mpu6050 import mpu6050
sensor = mpu6050(0x68) #0x68 is memory address on i2C
#Functions available
#.get_temp() -> temp in *C
#.get_gyro_data() -> Gyro data in param format. Access with sensor['x'] to get x value.
#.get_accel_data() -> Same as Gyro m/s^2
# If gyro
pwm = Adafruit_PCA9685.PCA9685()
#Values were deteremind to be the ZERO and MIN points for the proper PWM widths sent to onboard flight controller
#These have now been tested in the lab and are infact correct! Creates pulse widths beteen 1-2ms. Zero value = 1.5ms at 50hz
MIN_VALUE = 230
ZERO_VALUE = 325
#Frequency determind from someone who hacked the NAZAM-V2. Possible this could be changed. But it does work.
#Link here for reference: https://github.com/minla-rc/Minla-LTE-Receiver/raw/master/documentation/minla_pinout_and_naza_connection_v0.35_EN.pdf
pwm.set_pwm_freq(50)
# Output file for logging
current_output_name = time.strftime("%Y-%m-%d_%H:%M:%S", time.gmtime())
output_file = open("/home/pi/Adafruit_Python_PCA9685/examples/logs/%s.log" % (current_output_name), 'w')
# If system disconnects, this function is run to automatically land the Drone safely.
def disconnected(c, s):
printboth("Cannot Send... Must be disconnected from Controller... Resetting Speeds...")
talktopi(ZERO_VALUE, ZERO_VALUE, ZERO_VALUE, ZERO_VALUE)
time.sleep(1)
talktopi(ZERO_VALUE, ZERO_VALUE, MIN_VALUE, ZERO_VALUE)
c.close()
s.close()
def printboth(line):
sys.stdout.write("%s\n" % (line))
output_file.write("%s\n" % (line))
#A = Roll, E = Pitch, T = Throttle, R = Yaw
#Sends the proper PWM widths over the PWM board to the drone
def talktopi(aileron, elevator, throttle, rudder):
printboth("Roll: %s, Pitch: %s, Yaw: %s, Throttle: %s, "%(aileron, elevator, rudder, throttle))
pwm.set_pwm(0, 0, int(aileron)) #A
pwm.set_pwm(3, 0, int(elevator)) # E
pwm.set_pwm(4, 0, int(throttle)) # T
pwm.set_pwm(7, 0, int(rudder)) # R
time.sleep(0.01)
def Main():
#Local variable def
printboth("\n\n\n New Execution @%s....\n\n\n"%(time.strftime("%Y-%m-%d_%H:%M:%S", time.gmtime())))
values = []
# Auto grab host name (Drone IP and Port that it's listening on)
ip = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ip.connect(("8.8.8.8", 80))
host = ip.getsockname()[0]
port = 6970
#Listing properties -> Changed for VPN 0.0.0.0 (Allows all connections)
printboth("Server IP: 0.0.0.0")
printboth("Server PORT: %d"%(port))
s = socket.socket()
s.bind(('0.0.0.0', port))
#Waiting for connection from computer
s.listen(1)
c, addr = s.accept()
c.settimeout(3)
printboth("Connection from: " + str(addr))
while True:
#Trys to send data to Controller to maintain sync
try:
c.send('1')
except:
disconnected()
break
#Recieves data from Controller to grab current values
#Format ####,####,####,#### = 8 bits * 4 numbers * 4 pairs + 4 commas = 132 bits. Did 160 bits just to be safe
try:
data = c.recv(160).decode('utf-8')
except socket.error, e:
err = e.args[0]
if err == 'timed out':
disconnected(c, s)
break
else:
printboth(e)
break
# A = Roll, E = Pitch, T = Throttle, R = Ya
values = re.split(",+", data)
try:
talktopi(values[0], values[1], values[2], values[3])
except IndexError:
printboth("Index Error... Most likely connection down..")
disconnected(c, s)
break
c.close()
s.close()
#Go again
#Main()
if __name__ == '__main__':
# Ensures the program does not crash on the PI!
while True:
try:
Main()
time.sleep(5)
except socket.error, e:
try:
err = e.args[0]
except:
output_file.close()
print("KeyboardInterrupt... Output file closed...")
break
if err == 101:
printboth("Network is unreachable.. Connecting..")
time.sleep(10)
elif err == 98:
printboth("Socket is unavailable... Waiting")
time.sleep(10)
else:
printboth(">%s< &%s "%(err, e))
output_file.close()
print("Output file closed...")
break