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 __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()
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")
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()
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)