No Description

glitterpos.py 8.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276
  1. """glitter positioning system"""
  2. import time
  3. import gc
  4. import math
  5. import adafruit_lsm9ds1
  6. import adafruit_gps
  7. import adafruit_rfm9x
  8. import board
  9. import busio
  10. import digitalio
  11. import neopixel
  12. import rtc
  13. from glitterpos_util import timestamp, compass_bearing, bearing_to_pixel, map_range
  14. # glitterpos_cfg.py should be unique to each box, and formatted as follows:
  15. #
  16. # MY_ID = 0 # must be a unique integer
  17. # MAG_MIN = (-0.25046, -0.23506, -0.322)
  18. # MAG_MAX = (0.68278, 0.70882, 0.59654)
  19. # DECLINATION_RAD = 235.27 / 1000.0 # Black Rock City in radians
  20. #
  21. # From the CircuitPython REPL, use `import calibrate` to find values for
  22. # MAG_MIN and MAG_MAX.
  23. from glitterpos_cfg import MY_ID, MAG_MIN, MAG_MAX, DECLINATION_RAD
  24. # Colors for status lights, NeoPixel ring, etc.:
  25. RED = (255, 0, 0)
  26. YELLOW = (255, 150, 0)
  27. GREEN = (0, 255, 0)
  28. CYAN = (0, 255, 255)
  29. BLUE = (0, 0, 255)
  30. PURPLE = (180, 0, 255)
  31. # BOULDER_ID = 23
  32. # Color presets for each glitterpos_id:
  33. COLOR_LOOKUP = {
  34. 0: GREEN,
  35. 1: BLUE,
  36. 2: PURPLE,
  37. 3: YELLOW,
  38. 4: CYAN,
  39. 5: (100, 0, 255),
  40. 6: (0, 100, 200),
  41. 7: (100, 50, 100),
  42. # BOULDER_ID: RED
  43. }
  44. # You can add fixed points here:
  45. DEFAULT_BOX_COORDS = {
  46. # BOULDER_ID: (40.018258, -105.278457)
  47. }
  48. RADIO_FREQ_MHZ = 915.0
  49. CS = digitalio.DigitalInOut(board.D10)
  50. RESET = digitalio.DigitalInOut(board.D11)
  51. class GlitterPOS:
  52. """glitter positioning system"""
  53. def __init__(self):
  54. """configure sensors, radio, blinkenlights"""
  55. # Our id and the dict for storing coords of other glitterpos_boxes:
  56. self.glitterpos_id = MY_ID
  57. self.glitterpos_boxes = DEFAULT_BOX_COORDS
  58. # Set the RTC to an obviously bogus time for debugging purposes:
  59. # time_struct takes: (tm_year, tm_mon, tm_mday, tm_hour, tm_min, tm_sec, tm_wday, tm_yday, tm_isdst)
  60. rtc.RTC().datetime = time.struct_time((2000, 1, 1, 0, 0, 0, 0, 0, 0))
  61. print("startup time: " + timestamp())
  62. self.time_set = False
  63. self.last_send = time.monotonic()
  64. # A tuple for our lat/long:
  65. self.coords = (0, 0)
  66. self.heading = 0.0
  67. # Status light on the board, we'll use to indicate GPS fix, etc.:
  68. self.statuslight = neopixel.NeoPixel(board.NEOPIXEL, 1, brightness=0.005, auto_write=True)
  69. self.statuslight.fill(RED)
  70. # Neopixel ring:
  71. self.pixels = neopixel.NeoPixel(board.A1, 16, brightness=0.01, auto_write=False)
  72. self.startup_animation()
  73. time.sleep(2)
  74. self.init_radio()
  75. self.init_gps()
  76. self.init_compass()
  77. self.statuslight.fill(YELLOW)
  78. def startup_animation(self):
  79. """Initialize NeoPixel test pattern."""
  80. self.pixels[bearing_to_pixel(0)] = PURPLE
  81. self.pixels.show()
  82. time.sleep(.5)
  83. self.pixels[bearing_to_pixel(90)] = GREEN
  84. self.pixels.show()
  85. time.sleep(.5)
  86. self.pixels[bearing_to_pixel(180)] = YELLOW
  87. self.pixels.show()
  88. time.sleep(.5)
  89. self.pixels[bearing_to_pixel(270)] = BLUE
  90. self.pixels.show()
  91. def init_radio(self):
  92. """Set up RFM95."""
  93. spi = busio.SPI(board.SCK, MOSI=board.MOSI, MISO=board.MISO)
  94. self.rfm9x = adafruit_rfm9x.RFM9x(spi, CS, RESET, RADIO_FREQ_MHZ)
  95. self.rfm9x.tx_power = 18 # Default is 13 dB; the RFM95 goes up to 23 dB
  96. self.radio_tx('d', 'hello world')
  97. time.sleep(1)
  98. def init_gps(self):
  99. """Set up GPS module."""
  100. uart = busio.UART(board.TX, board.RX, baudrate=9600, timeout=3000)
  101. gps = adafruit_gps.GPS(uart)
  102. time.sleep(1)
  103. # https://cdn-shop.adafruit.com/datasheets/PMTK_A11.pdf
  104. # Turn on the basic GGA and RMC info (what you typically want), then
  105. # set update to once a second:
  106. gps.send_command('PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
  107. gps.send_command('PMTK220,1000')
  108. self.gps = gps
  109. def init_compass(self):
  110. """Set up LSM9DS1."""
  111. i2c = busio.I2C(board.SCL, board.SDA)
  112. self.compass = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
  113. time.sleep(1)
  114. def advance_frame(self):
  115. """
  116. Check the radio for new packets, poll GPS and compass data, send a
  117. radio packet if coordinates have changed (or if it's been a while), and
  118. update NeoPixel display. Called in an infinite loop by code.py.
  119. To inspect the state of the system, initialize a new GlitterPOS object
  120. from the CircuitPython REPL, and call gp.advance_frame() manually. You
  121. can then access the instance variables defined in __init__() and
  122. init_()* methods.
  123. """
  124. current = time.monotonic()
  125. self.radio_rx(timeout=0.5)
  126. new_gps_data = self.gps.update()
  127. self.update_heading()
  128. self.display_pixels()
  129. if not self.gps.has_fix:
  130. # Try again if we don't have a fix yet.
  131. self.statuslight.fill(RED)
  132. return
  133. # We want to send coordinates out either on new GPS data or roughly
  134. # every 15 seconds:
  135. if (not new_gps_data) and (current - self.last_send < 15):
  136. return
  137. # Set the RTC to GPS time (UTC):
  138. if new_gps_data and not self.time_set:
  139. rtc.RTC().datetime = self.gps.timestamp_utc
  140. self.time_set = True
  141. gps_coords = (self.gps.latitude, self.gps.longitude)
  142. if gps_coords == self.coords:
  143. return
  144. self.coords = (self.gps.latitude, self.gps.longitude)
  145. self.statuslight.fill(BLUE)
  146. print(':: ' + str(current)) # Print a separator line.
  147. print(timestamp())
  148. send_packet = '{}:{}:{}:{}'.format(
  149. self.gps.latitude,
  150. self.gps.longitude,
  151. self.gps.speed_knots,
  152. self.heading
  153. )
  154. print(' quality: {}'.format(self.gps.fix_quality))
  155. print(' ' + str(gc.mem_free()) + " bytes free")
  156. # Send a location packet:
  157. self.radio_tx('l', send_packet)
  158. def update_heading(self):
  159. mag_x, mag_y, mag_z = self.compass.magnetometer
  160. # print('Magnetometer: ({0:10.3f}, {1:10.3f}, {2:10.3f})'.format(mag_x, mag_y, mag_z))
  161. mag_x = map_range(mag_x, MAG_MIN[0], MAG_MAX[0], -1, 1)
  162. mag_y = map_range(mag_y, MAG_MIN[1], MAG_MAX[1], -1, 1)
  163. mag_z = map_range(mag_z, MAG_MIN[2], MAG_MAX[2], -1, 1)
  164. heading_mag = (math.atan2(mag_y, mag_x) * 180) / math.pi
  165. if heading_mag < 0:
  166. heading_mag = 360 + heading_mag
  167. # Account for declination (given in radians above):
  168. heading = heading_mag + (DECLINATION_RAD * 180 / math.pi)
  169. if heading > 360:
  170. heading = heading - 360
  171. print('heading: {}'.format(heading))
  172. self.heading = heading
  173. def radio_tx(self, msg_type, msg):
  174. """send a packet over radio with id prefix"""
  175. packet = 'e:' + msg_type + ':' + str(self.glitterpos_id) + ':' + msg
  176. print(' sending: ' + packet)
  177. # Blocking, max of 252 bytes:
  178. self.rfm9x.send(packet)
  179. self.last_send = time.monotonic()
  180. def radio_rx(self, timeout=0.5):
  181. """check radio for new packets, handle incoming data"""
  182. packet = self.rfm9x.receive(timeout)
  183. # If no packet was received during the timeout then None is returned:
  184. if packet is None:
  185. return
  186. packet = bytes(packet)
  187. print(timestamp())
  188. print(' received signal strength: {0} dB'.format(self.rfm9x.rssi))
  189. print(' received (raw bytes): {0}'.format(packet))
  190. pieces = packet.split(b':')
  191. if pieces[0] != b'e' or len(pieces) < 5:
  192. print(' bogus packet, bailing out')
  193. return
  194. msg_type = pieces[1].format()
  195. sender_id = int(pieces[2].format())
  196. # A location message:
  197. if msg_type == 'l':
  198. sender_lat = float(pieces[3].format())
  199. sender_lon = float(pieces[4].format())
  200. self.glitterpos_boxes[sender_id] = (sender_lat, sender_lon)
  201. # packet_text = str(packet, 'ascii')
  202. # print('Received (ASCII): {0}'.format(packet_text))
  203. def display_pixels(self):
  204. """Display current state on the NeoPixel ring."""
  205. self.pixels.fill((0, 0, 0))
  206. # We can't meaningfully point at other locations if we don't know our
  207. # own position:
  208. if not self.gps.has_fix:
  209. return
  210. for box in self.glitterpos_boxes:
  211. bearing_to_box = compass_bearing(self.coords, self.glitterpos_boxes[box])
  212. # Treat current compass heading as our origin point for display purposes:
  213. display_bearing = bearing_to_box - self.heading
  214. if display_bearing < 0:
  215. display_bearing = display_bearing + 360
  216. pixel = bearing_to_pixel(display_bearing)
  217. # print('display pixel: {}'.format(pixel))
  218. color = (15, 15, 15)
  219. if box in COLOR_LOOKUP:
  220. color = COLOR_LOOKUP[box]
  221. self.pixels[pixel] = color
  222. self.pixels.show()