Merge branch 'main' into 60-base-sensors

EosPayload/drivers/GPS_driver.py

@@ -1,5 +1,4 @@
 import logging
-import time
 import queue
 import adafruit_gps
 import serial
@@ -34,171 +33,85 @@ def __init__(self, output_directory: str):
 
     def setup(self) -> None:
         super().setup()
+        self.register_thread('device-read', self.device_read)
+
         UART.setup("UART1")
         self.uart = serial.Serial(port="/dev/ttyO1", baudrate=9600)
         self.gps = adafruit_gps.GPS(self.uart, debug=False)
 
         self.gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
         self.gps.send_command(b"PMTK220,1000")
 
-    @staticmethod
-    def read_thread_enabled() -> bool:
-        return True
-
-    @staticmethod
-    def command_thread_enabled() -> bool:
-        return False # CHANGE
-
     def device_read(self, logger: logging.Logger) -> None:
 
-        last_print = time.monotonic()
         while True:
             self.gps.update()
-            current = time.monotonic()
-            if current - last_print >= 1.0:
-                last_print = current
-                if not self.gps.has_fix:
-                    # Try again if we don't have a fix yet.
-                    logger.info("Waiting for fix...")
-                    continue
-
-                logger.info("=" * 40)
-                time_hr = self.gps.timestamp_utc.tm_hour
-                time_min = self.gps.timestamp_utc.tm_min
-                time_sec = self.gps.timestamp_utc.tm_sec
-                time_day = self.gps.timestamp_utc.tm_mday
-                time_month = self.gps.timestamp_utc.tm_mon
-                time_year = self.gps.timestamp_utc.tm_year
-                logger.info(
-                    "Fix timestamp: {}/{}/{} {:02}:{:02}:{:02}".format(
-                        time_month,  # Grab parts of the time from the
-                        time_day,  # struct_time object that holds
-                        time_year,  # the fix time.  Note you might
-                        time_hr,  # not get all data like year, day,
-                        time_min,  # month!
-                        time_sec,
-                    )
+            if not self.gps.has_fix:
+                # Try again if we don't have a fix yet.
+                logger.info("Waiting for fix...")
+                self.thread_sleep(logger, 1)
+                continue
+
+            logger.info("=" * 40)
+            time_hr = self.gps.timestamp_utc.tm_hour
+            time_min = self.gps.timestamp_utc.tm_min
+            time_sec = self.gps.timestamp_utc.tm_sec
+            time_day = self.gps.timestamp_utc.tm_mday
+            time_month = self.gps.timestamp_utc.tm_mon
+            time_year = self.gps.timestamp_utc.tm_year
+            logger.info(
+                "Fix timestamp: {}/{}/{} {:02}:{:02}:{:02}".format(
+                    time_month,  # Grab parts of the time from the
+                    time_day,  # struct_time object that holds
+                    time_year,  # the fix time.  Note you might
+                    time_hr,  # not get all data like year, day,
+                    time_min,  # month!
+                    time_sec,
                 )
-
-                gps_lat = self.gps.latitude
-                gps_lon = self.gps.longitude
-                gps_alt = self.gps.altitude_m
-                gps_speed = self.gps.speed_knots
-                gps_sat = self.gps.satellites
-
-                logger.info("Latitude: {0:.6f} degrees".format(self.gps.latitude))
-                logger.info("Longitude: {0:.6f} degrees".format(self.gps.longitude))
-                if self.gps.altitude_m is not None:
-                    logger.info("Altitude: {} meters".format(self.gps.altitude_m))
-
-                # time
-                try:
-                    # "%H:%M:%S %d/%m/%Y"
-                    data_datetime_string = "{:02}:{:02}:{:02} {}/{}/{}".format(time_hr, time_min, time_sec, time_day,
-                                                                               time_month, time_year)
-                    data_datetime = datetime.datetime.strptime(data_datetime_string, GPSDriver.data_time_format)
-                    date_time = str(data_datetime.timestamp())
-                except Exception:
-                    date_time = str(datetime.datetime.now())
-
-                logger.info(date_time)
-
-                position_bytes = Position.encode_position(float(date_time), float(gps_lat),
-                                                          float(gps_lon), float(gps_alt),
-                                                          float(gps_speed), int(gps_sat),
-                                                          self.current_flight_state)
-
-                gps_packet = Packet(position_bytes, DataHeader(Device.GPS, Type.POSITION, Priority.TELEMETRY))
-
-                if self.gotten_first_fix is False:
-                    position = Position.decode_position(gps_packet)
-                    if position.valid:
-                        self.gotten_first_fix = True
-                        logger.info("Got first GPS fix")
-
-                self._mqtt.send(Topic.POSITION_UPDATE, gps_packet.encode())
-                if datetime.datetime.now() - self.last_transmit_time > self.transmit_rate:
-                    self._mqtt.send(Topic.RADIO_TRANSMIT, gps_packet.encode())
-                    self.last_transmit_time = datetime.datetime.now()
-
-    def device_command(self, logger: logging.Logger) -> None:
-        last_emit_time = datetime.datetime.now()
-        last_state_update_time = datetime.datetime.now()
-        logger.info("Starting to poll for data!")
-        while True:
-            #incoming_raw_data = self.read_queue.get()
-            #incoming_processed_data, incoming_data_dict = self.process_raw_esp_data(incoming_raw_data)
-            #incoming_processed_data.append(str(self.gotten_first_fix))
-            #self.data_log(incoming_processed_data)
-            if (datetime.datetime.now() - last_emit_time) > self.emit_rate:
-                last_emit_time = datetime.datetime.now()
-            #    self.emit_data(incoming_data_dict, logger)
-            if (datetime.datetime.now() - last_state_update_time) > self.state_update_rate:
-                if self.old_position is None or self.latest_position.local_time is None:
-                    self.old_position = self.latest_position
-                elif (datetime.datetime.now() - self.latest_position.local_time) > self.position_timeout:
-                    self.current_flight_state = FlightState.UNKNOWN
-                elif self.latest_position.altitude < 1000:
-                    self.current_flight_state = FlightState.ON_GROUND
-                elif self.latest_position.altitude >= self.old_position.altitude:
-                    self.current_flight_state = FlightState.ASCENT
-                elif self.latest_position.altitude < self.old_position.altitude:
-                    self.current_flight_state = FlightState.DESCENT
-                else:
-                    self.current_flight_state = FlightState.UNKNOWN
-                self.old_position = self.latest_position
-                last_state_update_time = datetime.datetime.now()
+            )
+
+            gps_lat = self.gps.latitude
+            gps_lon = self.gps.longitude
+            gps_alt = self.gps.altitude_m
+            gps_speed = self.gps.speed_knots
+            gps_sat = self.gps.satellites
+
+            logger.info("Latitude: {0:.6f} degrees".format(self.gps.latitude))
+            logger.info("Longitude: {0:.6f} degrees".format(self.gps.longitude))
+            if self.gps.altitude_m is not None:
+                logger.info("Altitude: {} meters".format(self.gps.altitude_m))
+
+            # time
+            try:
+                # "%H:%M:%S %d/%m/%Y"
+                data_datetime_string = "{:02}:{:02}:{:02} {}/{}/{}".format(time_hr, time_min, time_sec, time_day,
+                                                                           time_month, time_year)
+                data_datetime = datetime.datetime.strptime(data_datetime_string, GPSDriver.data_time_format)
+                date_time = str(data_datetime.timestamp())
+            except Exception:
+                date_time = str(datetime.datetime.now())
+
+            logger.info(date_time)
+
+            position_bytes = Position.encode_position(float(date_time), float(gps_lat),
+                                                      float(gps_lon), float(gps_alt),
+                                                      float(gps_speed), int(gps_sat),
+                                                      self.current_flight_state)
+
+            gps_packet = Packet(position_bytes, DataHeader(Device.GPS, Type.POSITION, Priority.TELEMETRY))
+
+            if self.gotten_first_fix is False:
+                position = Position.decode_position(gps_packet)
+                if position.valid:
+                    self.gotten_first_fix = True
+                    logger.info("Got first GPS fix")
+
+            self._mqtt.send(Topic.POSITION_UPDATE, gps_packet.encode())
+            if datetime.datetime.now() - self.last_transmit_time > self.transmit_rate:
+                self._mqtt.send(Topic.RADIO_TRANSMIT, gps_packet.encode())
+                self.last_transmit_time = datetime.datetime.now()
+
+            self.thread_sleep(logger, 1)
 
     def cleanup(self):
         self.uart.close()
-
-
-
-'''
-def gps_converter(lat, lon, track_history):
-    """ Rounds the lat/lon from GPS data and creates the list of coordinates that shows the previous route taken
-    this removes the last 15 data points due to those not being as smoothed as earlier points"""
-    try:
-        lat = round(lat,5)
-        lon = round(lon,5)
-
-        noduplicates = track_history[-10:]
-        if not [lon, lat] in noduplicates:
-            track_history.append([lon, lat])
-
-        outline = filter(track_history[-15:])
-        track_history = track_history[:-15]
-
-        for item in outline:
-            item[0] = round(item[0],5)
-            item[1] = round(item[1],5)
-            track_history.append(item)
-        lat = track_history[-5][1]
-        lon = track_history[-5][0]
-        return lat, lon, track_history
-    except:
-        return lat, lon, track_history
-
-        #while (1):
-        #    while GPS.inWaiting() == 0:
-        #        pass
-        #    NMEA = GPS.readline()
-        #    logger.info(NMEA)
-'''
-'''
-        logger.info("PIN ||||| 1")
-        uart = busio.UART(board.TX, board.RX, baudrate=9600, timeout=10)
-        logger.info("PIN ||||| 2")
-        gps = adafruit_gps.GPS(uart, debug=False)  # Use UART/pyserial
-        logger.info("PIN ||||| 3")
-
-        gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
-        gps.send_command(b"PMTK220,1000")
-        logger.info("PIN ||||| 4")
-
-        while True:
-            gps.update()
-            logger.info("PING X")
-            time.sleep(1)
-
-'''

EosPayload/drivers/RadioDriver.py

@@ -16,7 +16,6 @@
 from EosPayload.lib.base_drivers.driver_base import DriverBase
 from EosPayload.lib.mqtt import Topic
 
-
 class RadioDriver(DriverBase):
     _thread_queue = PriorityQueue()
     sequence_number = 0
@@ -30,6 +29,9 @@ class RadioDriver(DriverBase):
 
     def setup(self) -> None:
         super().setup()
+        self.register_thread('device-read', self.device_read)
+        self.register_thread('device-command', self.device_command)
+
         serial_id = "FTDI_XBIB-XBP9XR-0_FT5PG7VE"
         context = pyudev.Context()
         devices = context.list_devices(ID_SERIAL=serial_id)
@@ -68,19 +70,8 @@ def setup(self) -> None:
                 self._logger.error(f"radio port not open: {e}")
                 time.sleep(10)
 
-    @staticmethod
-    def read_thread_enabled() -> bool:
-        return True
-
-    @staticmethod
-    def command_thread_enabled() -> bool:
-        return True
-
-    @staticmethod
-    def enabled() -> bool:
-        return True
-
     def device_read(self, logger: logging.Logger) -> None:
+        # TODO: refactor to move this stuff to setup, a separate thread is pointless
         # Receives data from radio and sends it to MQTT
         def data_receive_callback(xbee_message):
             packet = xbee_message.data  # raw bytearray packet
@@ -115,11 +106,12 @@ def xbee_send_callback(_client, _userdata, message):
 
         self._mqtt.register_subscriber(Topic.RADIO_TRANSMIT, xbee_send_callback)
         self.port.add_data_received_callback(data_receive_callback)
-        self.spin()
+        self.thread_spin(logger)
 
     # queue thread stuff
     def device_command(self, logger: logging.Logger) -> None:
         while True:
+            # TODO: refactor to be non-blocking so stop signal can be checked
             (priority, timestamp, packet) = self._thread_queue.get()
             logger.info(f":: = {packet.body}")
             try:

EosPayload/drivers/camera_driver.py

@@ -9,10 +9,6 @@
 
 class Camera1Driver(DriverBase):
 
-    @staticmethod
-    def read_thread_enabled() -> bool:
-        return True
-
     def video_writer_setup(self):
         return cv2.VideoWriter(os.path.join(self.path, self.video_name_format.format(self.video_num)),
                                self.fourcc, self.camera_fps, self.camera_res)
@@ -44,6 +40,9 @@ def __init__(self, output_directory: str, config: dict):
         self.still_num = 0
 
     def setup(self) -> None:
+        super().setup()
+        self.register_thread('device-read', self.device_read)
+
         self.still_name_format = "camera-{camera}-still-image-{num}.jpg".format(camera=self.camera_num, num='{}')
         self.video_name_format = "camera-{camera}-video-{num}.avi".format(camera=self.camera_num, num='{}')
         self.video_num = self.find_next_file_num(self.video_name_format)
@@ -93,7 +92,7 @@ def device_read(self, logger: logging.Logger) -> None:
                 video_start_time = datetime.datetime.now()
                 self.out.release()
                 self.out = self.video_writer_setup()
-            time.sleep(1/self.camera_fps)
+            self.thread_sleep(logger, 1/self.camera_fps)
             ret, frame = self.cap.read()
             if not ret:
                 logger.warning("Video frame capture failed")

EosPayload/drivers/led_driver.py

@@ -1,22 +1,19 @@
 import Adafruit_BBIO.GPIO as GPIO
 import logging
-import time
 
 from EosPayload.lib.base_drivers.driver_base import DriverBase
 
 
 class LEDDriver(DriverBase):
 
-    @staticmethod
-    def command_thread_enabled() -> bool:
-        return True
-
     def __init__(self, output_directory: str, config: dict):
         super().__init__(output_directory, config)
         self.pin_name = "P9_12"
 
     def setup(self) -> None:
         super(LEDDriver, self).setup()
+        self.register_thread('device-command', self.device_command)
+
         GPIO.setup(self.pin_name, GPIO.OUT)
 
     def device_command(self, logger: logging.Logger) -> None:
@@ -27,7 +24,7 @@ def device_command(self, logger: logging.Logger) -> None:
                 pin_state = 1
             else:
                 pin_state = 0
-            time.sleep(5)
+            self.thread_sleep(logger, 5)
 
     def cleanup(self):
         super(LEDDriver, self).cleanup()

EosPayload/drivers/ping_driver.py

@@ -1,6 +1,5 @@
 from enum import Enum, unique
 import logging
-import time
 import traceback
 
 from EosLib import Priority, Type
@@ -33,9 +32,9 @@ class Commands(str, Enum):
         ACK = "ACK"
         ERR = "ERR"
 
-    @staticmethod
-    def command_thread_enabled() -> bool:
-        return True
+    def setup(self) -> None:
+        super().setup()
+        self.register_thread('device-command', self.device_command)
 
     def device_command(self, logger: logging.Logger) -> None:
         if self._mqtt:
@@ -45,7 +44,7 @@ def device_command(self, logger: logging.Logger) -> None:
         while True:
             self.ping_ground(counter, logger)
             counter = counter + 1
-            time.sleep(60)
+            self.thread_sleep(logger, 60)
 
     def ping_reply(self, client, user_data, message):
         try: