Exemple #1
0
 def __init__(self):
     super().__init__('gps')
     # setup the GPS
     self.declare_parameter('gps_baud', self.BAUDRATE)
     self.declare_parameter('gps_port', self.PORT)
     self.baud = self.get_parameter('gps_baud').value
     self.port = self.get_parameter('gps_port').value
     self.ubp = GPSReader(self.port, self.baud, 
             self.TIMEOUT, self.UBXONLY)
     if not self.ubp.connect():
         # TODO: raise custom error
         self.get_logger().error(f"GPS Failed to connect on given port. Aborting")
         return
     self.ubp.config_timepulse_badly()
     self.ubp.config_msg(self.ON)
     # setup the ROS
     self.declare_parameter('gps_top', 'gps')
     self.declare_parameter('time_top', 'gps_time')
     pub_top = self.get_parameter('gps_top').value
     time_top = self.get_parameter('time_top').value
     self.fix_pub = self.create_publisher(NavSatFix, pub_top, 10)
     self.time_pub = self.create_publisher(TimeReference, time_top, 10)
     # setup the gpio
     GPIO.setmode(GPIO.BCM)
     GPIO.setup(26, GPIO.IN, GPIO.PUD_UP)
     GPIO.add_event_detect(26, GPIO.RISING, 
         callback=self.timepulse_callback, bouncetime=50)
Exemple #2
0
def main(protocol):
    try:
        print("\nPython %s\n" % sys.version)
        print("IoT Hub Client for Python")

        hub_manager = HubManager(protocol)

        print("Starting the GPS locator module using protocol %s..." %
              hub_manager.client_protocol)
        print(
            "This module is now waiting for messages and will indefinitely.  Press Ctrl-C to exit. "
        )

        #start reading gps data...
        myGPSReader = GPSReader(GPSDeviceTtyAC)
        latestFixedTime = None

        while True:
            latestFixedPoint = myGPSReader.getLatestFixedGPSPoint()
            if latestFixedPoint is not None and latestFixedPoint.sentenceTimestamp != latestFixedTime:
                #new GPS point than previous one, send to iot hub
                jsonPayload = latestFixedPoint.buildJsonPayload(DEVICE_ID)
                print("Payload of new GPS data is: " + jsonPayload)

                msg = IoTHubMessage(bytearray(jsonPayload, 'utf8'))

                #in order to use IoT Hub message routing on body, we have to setup the content type and encoding
                set_content_result = msg.set_content_encoding_system_property(
                    "utf-8")
                set_content_type_result = msg.set_content_type_system_property(
                    "application/json")

                if set_content_result != 0:
                    print("set_content_encoding_system_property FAILED")

                if set_content_type_result != 0:
                    print("set_content_type_system_property FAILED")

                hub_manager.forward_event_to_output("gps", msg, 0)
                latestFixedTime = latestFixedPoint.sentenceTimestamp
            else:
                print(str(datetime.now()) + ": Not fixed yet....")
            time.sleep(1)

    except IoTHubError as iothub_error:
        print("Unexpected error %s from IoTHub" % iothub_error)
        return
    except KeyboardInterrupt:
        print("IoTHubModuleClient sample stopped")
Exemple #3
0
    def timepulse_callback(self, channel):
        self.get_logger().info(f"{time.time()} Timepulse trigger")
        gps_msg = NavSatFix()
        timeref_msg = TimeReference()
        msg_hdr = Header()
        system_time = self.get_clock().now().to_msg()
        msg_hdr.frame_id = 'base_link' # center of the plane
        try:
            ubx = self.ubp.read()
        except IOError:
            self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
            self.ubp = GPSReader(self.port, self.baud, 
                    self.TIMEOUT, self.UBXONLY)
            return
        while ubx:
            if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
                # <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>

                msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)

                fix_stat = NavSatStatus()

                if ubx.fixType == 0:
                    self.get_logger().warning(f"No fix yet.")
                    break

                fix_stat.service = SERVICE_GPS

                gps_msg.status = fix_stat
                gps_msg.header = msg_hdr
                gps_msg.latitude = float(ubx.lat)/10000000
                gps_msg.longitude = float(ubx.lon)/10000000
                gps_msg.altitude = float(ubx.height)/1000

                timeref_msg.header = msg_hdr
                timeref_msg.time_ref = system_time
                timeref_msg.source = "GPS"

                self.fix_pub.publish(gps_msg)
                self.time_pub.publish(timeref_msg)

                self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
                return
            else: 
                self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
                ubx = self.ubp.read()
    def __init__(self):
        """Initializes the DataCollector."""
        super(DataCollector, self).__init__()
        
        obd2readerClass  = getattr(obd2reader, CONFIGURATION["obd2reader"])
        self._obd2reader = obd2readerClass(CONFIGURATION["device"], CONFIGURATION["speed"])
        self._obd2reader.open_connection()

        self._gpsreader  = GPSReader(CONFIGURATION["gpsdPort"])
Exemple #5
0
    def __init__(self, display):
        """Initializes the DataCollector.

        :param Display display: Display that can be used to view messages.
        """
        super(DataCollector, self).__init__()

        obd2readerClass = getattr(obd2reader, CONFIGURATION["obd2reader"])
        self._obd2reader = obd2readerClass(CONFIGURATION["device"],
                                           CONFIGURATION["speed"], display)
        self._obd2reader.open_connection()

        self._gpsreader = GPSReader()
Exemple #6
0
class Gps(Node):
    ON = b"\x00\x01\x01\x01\x00\x00"
    OFF = b"\x00\x00\x00\x00\x00\x00"
    PORT = "/dev/ttyACM0"
    BAUDRATE = 9600
    TIMEOUT = 1
    UBXONLY = False
    def __init__(self):
        super().__init__('gps')
        # setup the GPS
        self.declare_parameter('gps_baud', self.BAUDRATE)
        self.declare_parameter('gps_port', self.PORT)
        self.baud = self.get_parameter('gps_baud').value
        self.port = self.get_parameter('gps_port').value
        self.ubp = GPSReader(self.port, self.baud, 
                self.TIMEOUT, self.UBXONLY)
        if not self.ubp.connect():
            # TODO: raise custom error
            self.get_logger().error(f"GPS Failed to connect on given port. Aborting")
            return
        self.ubp.config_timepulse_badly()
        self.ubp.config_msg(self.ON)
        # setup the ROS
        self.declare_parameter('gps_top', 'gps')
        self.declare_parameter('time_top', 'gps_time')
        pub_top = self.get_parameter('gps_top').value
        time_top = self.get_parameter('time_top').value
        self.fix_pub = self.create_publisher(NavSatFix, pub_top, 10)
        self.time_pub = self.create_publisher(TimeReference, time_top, 10)
        # setup the gpio
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(26, GPIO.IN, GPIO.PUD_UP)
        GPIO.add_event_detect(26, GPIO.RISING, 
            callback=self.timepulse_callback, bouncetime=50)

    def timepulse_callback(self, channel):
        self.get_logger().info(f"{time.time()} Timepulse trigger")
        gps_msg = NavSatFix()
        timeref_msg = TimeReference()
        msg_hdr = Header()
        system_time = self.get_clock().now().to_msg()
        msg_hdr.frame_id = 'base_link' # center of the plane
        try:
            ubx = self.ubp.read()
        except IOError:
            self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
            self.ubp = GPSReader(self.port, self.baud, 
                    self.TIMEOUT, self.UBXONLY)
            return
        while ubx:
            if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
                # <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>

                msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)

                fix_stat = NavSatStatus()

                if ubx.fixType == 0:
                    self.get_logger().warning(f"No fix yet.")
                    break

                fix_stat.service = SERVICE_GPS

                gps_msg.status = fix_stat
                gps_msg.header = msg_hdr
                gps_msg.latitude = float(ubx.lat)/10000000
                gps_msg.longitude = float(ubx.lon)/10000000
                gps_msg.altitude = float(ubx.height)/1000

                timeref_msg.header = msg_hdr
                timeref_msg.time_ref = system_time
                timeref_msg.source = "GPS"

                self.fix_pub.publish(gps_msg)
                self.time_pub.publish(timeref_msg)

                self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
                return
            else: 
                self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
                ubx = self.ubp.read()

    def _gen_timestamp_from_utc(self, ubx):
        second = ubx.second
        usecond = ubx.nano/1000
        subtract_sec = False
        if usecond < 0:
            usecond = 1000000 + usecond
            subtract_sec = True
        utc = datetime.datetime(ubx.year, ubx.month, ubx.day, ubx.hour, ubx.min, second, int(usecond))
        gps_time = utc.timestamp()
        if subtract_sec:
            gps_time -= 1
        gps_stamp = self.get_clock().now().to_msg()
        gps_stamp.sec = int(gps_time)
        gps_stamp.nanosec = int((gps_time - int(gps_time))*1000000000)
        return gps_stamp
class DataCollector(object):
    """The DataCollector collects and stores all data.

    The stored data includes:
        - Time
        - GPS data
        - OBD2 data
    """

    def __init__(self):
        """Initializes the DataCollector."""
        super(DataCollector, self).__init__()
        
        obd2readerClass  = getattr(obd2reader, CONFIGURATION["obd2reader"])
        self._obd2reader = obd2readerClass(CONFIGURATION["device"], CONFIGURATION["speed"])
        self._obd2reader.open_connection()

        self._gpsreader  = GPSReader(CONFIGURATION["gpsdPort"])

    def shutdown(self):
        """Shuts down the DataCollector and all of its Threads."""
        self._obd2reader.shutdown()
        self._gpsreader.shutdown()

    def write_data_log(self, logFileName, nbrOfOBDFrames, messagesPerTimestamp):
        """Stores an data log file to the given location.

        :param String  logFileName:             Path to store the log file.
        :param Integer nbrOfOBDFrames:          Number of OBD frames to be stored inside the log.
                                                This parameter does NOT affect the number of time
                                                stamps or GPS reports.
        :param Integer messagesPerTimestamp:    Number of OBD2 frames share the same time stamp.

        :return:    Returns the number of bytes stored in the log.
        :rtype:     Integer
        """
        ## create the log file
        logf  = file(logFileName, "ab")
        
        ## performance optimization
        ## reduces the __getattr__ calls for the major instances
        write      = logf.write
        gpsreader  = self._gpsreader
        obd2reader = self._obd2reader

        ## number of bytes written
        bytes = 0

        ## collect the requested number of OBD2 frames
        for idx in xrange(0, nbrOfOBDFrames, messagesPerTimestamp):

            ## time stamp
            timestamp = "#TIME__%s\n" % _getCurrentTime()
            write(timestamp)
            bytes += len(timestamp)

            ## read the number of frames requested
            try:
                logEntries = obd2reader.read_frames(nbrOfOBDFrames)
            except:
                obd2reader.reconnect()
                continue
            
            ## store the frames in the log
            logEntries = "\n".join(logEntries)
            write("%s\n" % logEntries)
            bytes += len(logEntries) + 1

            ## append the gpslog entries, when there are any
            gpsreports = ["#GPS__%s\n" % entry for entry in gpsreader.get_current_gps_entries()]
            
            ## no GPS report collected since last run
            if not gpsreports:
                continue

            ## store the reports in the log
            gpsreports = "\n".join([str(r) for r in gpsreports])
            write("%s\n" % gpsreports)
            bytes += len(gpsreports) + 1


        # close the log file
        logf.close()
        
        ## return the number of bytes written
        return bytes
Exemple #8
0
from gpsreader import GPSReader 
import time
import json
from datetime import datetime

#start reading gps data...
myGPSReader = GPSReader("/dev/ttyACM0")
latestFixedTime = None

while True:
    latestFixedPoint = myGPSReader.getLatestFixedGPSPoint()
    if latestFixedPoint is not None and latestFixedPoint.sentenceTimestamp != latestFixedTime:
        #new GPS point than previous one, send to iot hub
        jsonPayload = latestFixedPoint.buildJsonPayload("test_device")
        print("Payload of new GPS data is: " + jsonPayload)
        latestFixedTime = latestFixedPoint.sentenceTimestamp
        print("++++++++++++++++++++")
    else:
        print(str(datetime.now()) + ": Not fixed yet....")
    time.sleep(1)