-
Notifications
You must be signed in to change notification settings - Fork 36
/
camera.py
executable file
·212 lines (175 loc) · 7.17 KB
/
camera.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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#!/usr/bin/env python3
import argparse
import threading
import json
import sys
import os
import logging
import logging
import coloredlogs
import calendar
from datetime import datetime, timedelta
import signal
import random
import time
import re
import errno
import paho.mqtt.client as mqtt
from json.decoder import JSONDecodeError
import pantilthat
from picamera import PiCamera
ID = str(random.randint(1,100001))
camera = PiCamera()
tiltCorrect = 15
args = None
pan = 0
tilt = 0
actualPan = 0
actualTilt = 0
currentPlane=0
# https://stackoverflow.com/questions/45659723/calculate-the-difference-between-two-compass-headings-python
# The camera hat takes bearings between -90 and 90.
# h1 is the target heading
# h2 is the heading the camera is pointed at
def getHeadingDiff(h1, h2):
if h1 > 360 or h1 < 0 or h2 > 360 or h2 < 0:
raise Exception("out of range")
diff = h1 - h2
absDiff = abs(diff)
if absDiff == 180:
return absDiff
elif absDiff < 180:
return diff
elif h2 > h1:
return 360 - absDiff
else:
return absDiff - 360
def setPan(bearing):
global pan
camera_bearing = args.bearing
diff_heading = getHeadingDiff(bearing, camera_bearing)
if diff_heading > -85 and diff_heading < 85:
if abs(pan - diff_heading) > 2:
logging.info("Heading Diff %d for Bearing %d & Camera Bearing: %d"% (diff_heading, bearing, camera_bearing))
pan = diff_heading
logging.info("Setting Pan to: %d"%pan)
return True
return False
def setTilt(elevation):
global tilt
if elevation < 90:
if abs(tilt-elevation) > 2:
tilt = elevation
logging.info("Setting Tilt to: %d"%elevation)
def moveCamera():
global actualPan
global actualTilt
global camera
while True:
lockedOn = False
if actualTilt != tilt:
logging.info("Moving Tilt to: %d Goal: %d"%(actualTilt, tilt))
if actualTilt < tilt:
actualTilt += 1
else:
actualTilt -= 1
if actualTilt == tilt:
lockedOn = True
if actualPan != pan:
logging.info("Moving Pan to: %d Goal: %d"%(actualPan, pan))
if actualPan < pan:
actualPan += 1
else:
actualPan -= 1
if actualPan == pan:
lockedOn = True
if lockedOn == True:
filename = "capture/{}_{}".format(datetime.now().strftime('%Y-%m-%d-%H-%M-%S'), currentPlane)
camera.capture("{}.jpeg".format(filename))
# Turns out that negative numbers mean to move the right and positive numbers mean move to the left...
# I think this is backwards, I am going to switch it, so here I am going to multiply by -1
pantilthat.pan(actualPan * -1)
# Same thing with the tilt. A negative angle moves the camera head up, a positive value down. Backwards!
# Multiplying by -1 again to make it normal. The camera is also off by a little and pointed up a bit, moving it down 20 degrees seems about right
pantilthat.tilt(actualTilt * -1 + tiltCorrect)
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.005)
#############################################
## MQTT Callback Function ##
#############################################
def on_message(client, userdata, message):
global currentPlane
command = str(message.payload.decode("utf-8"))
#rint(command)
try:
update = json.loads(command)
#payload = json.loads(messsage.payload) # you can use json.loads to convert string to json
except JSONDecodeError as e:
# do whatever you want
print(e)
except TypeError as e:
# do whatever you want in this case
print(e)
except ValueError as e:
print(e)
except:
print("Caught it!")
#logging.info("Bearing: {} Elevation: {}".format(update["bearing"],update["elevation"]))
bearingGood = setPan(update["bearing"])
setTilt(update["elevation"])
currentPlane = update["icao24"]
def main():
global args
global logging
global pan
global tilt
global camera
parser = argparse.ArgumentParser(description='An MQTT based camera controller')
parser.add_argument('-b', '--bearing', help="What bearing is the font of the PI pointed at (0-360)", default=0)
parser.add_argument('-m', '--mqtt-host', help="MQTT broker hostname", default='127.0.0.1')
parser.add_argument('-t', '--mqtt-topic', help="MQTT topic to subscribe to", default="SkyScan")
parser.add_argument('-v', '--verbose', action="store_true", help="Verbose output")
args = parser.parse_args()
level = logging.DEBUG if args.verbose else logging.INFO
styles = {'critical': {'bold': True, 'color': 'red'}, 'debug': {'color': 'green'}, 'error': {'color': 'red'}, 'info': {'color': 'white'}, 'notice': {'color': 'magenta'}, 'spam': {'color': 'green', 'faint': True}, 'success': {'bold': True, 'color': 'green'}, 'verbose': {'color': 'blue'}, 'warning': {'color': 'yellow'}}
level = logging.DEBUG if '-v' in sys.argv or '--verbose' in sys.argv else logging.INFO
if 1:
coloredlogs.install(level=level, fmt='%(asctime)s.%(msecs)03d \033[0;90m%(levelname)-8s '
''
'\033[0;36m%(filename)-18s%(lineno)3d\033[00m '
'%(message)s',
level_styles = styles)
else:
# Show process name
coloredlogs.install(level=level, fmt='%(asctime)s.%(msecs)03d \033[0;90m%(levelname)-8s '
'\033[0;90m[\033[00m \033[0;35m%(processName)-15s\033[00m\033[0;90m]\033[00m '
'\033[0;36m%(filename)s:%(lineno)d\033[00m '
'%(message)s')
logging.info("---[ Starting %s ]---------------------------------------------" % sys.argv[0])
pantilthat.pan(pan)
pantilthat.tilt(tilt)
camera.resolution = (1024, 768)
threading.Thread(target = moveCamera, daemon = True).start()
# Sleep for a bit so we're not hammering the HAT with updates
time.sleep(0.005)
print("connecting to MQTT broker at "+ args.mqtt_host+", channel '"+args.mqtt_topic+"'")
client = mqtt.Client("pan-tilt-pi-camera-" + ID) #create new instance
client.on_message=on_message #attach function to callback
client.connect(args.mqtt_host) #connect to broker
client.loop_start() #start the loop
client.subscribe(args.mqtt_topic+"/#")
client.publish("skyscan/registration", "pan-tilt-pi-camera-"+ID+" Registration", 0, False)
#############################################
## Main Loop ##
#############################################
timeHeartbeat = 0
while True:
if timeHeartbeat < time.mktime(time.gmtime()):
timeHeartbeat = time.mktime(time.gmtime()) + 10
client.publish("Heartbeat", "pan-tilt-pi-camera-"+ID+" Heartbeat", 0, False)
time.sleep(0.1)
if __name__ == "__main__":
try:
main()
except Exception as e:
logging.critical(e, exc_info=True)