bexus-26-imu-fusion-flight-software/groundstation/listenUDP.py

#!/usr/bin/python3
# -*- coding: utf-8 -*-

import socket
import time
import pprint
import sys

import datetime

from PyQt5.QtWidgets import *
from PyQt5.QtCore import (QThread, pyqtSignal, pyqtSlot)

from functions import *

pp = pprint.PrettyPrinter(indent=4)
dataset = """
/*-------RAW-----------*/
	uint32_t pressure_cis_raw;    // pack 1 x 4
	uint32_t temperature_cis_raw; //    + 1 x 4
	
	uint32_t pressure_spi_raw;
	uint32_t temperature_i2c_raw;
	
	uint32_t temperature_spi_raw;
	uint32_t adc_temperature_raw;
	
	int32_t  temperature_mpu_raw;
	uint32_t adc_temperature_internal_raw;
	
	uint32_t	adc_voltage_raw;
	uint32_t	adc_current_raw;
	
	struct vector_Uint accel_raw; // pack 3 x 4
	struct vector_Uint gyro_raw;  //    + 3 x 4
	
	struct vector_Uint mag_raw; // pack 3 x 4
	                            //    + 1 x 4
  int32_t temperature_mag_raw;
	
	/*-----Computed--------*/
	
	double pressure_cis; // no packing needed 1 x 8
	
	double temperature_cis;	
	
	double pressure_spi;	
	
	double temperature_i2c;	
	
	double temperature_spi;	
	
	double adc_temperature;	
	
	double adc_temperature_internal;			
	
	double adc_voltage;	

	double adc_current;		

	double temperature_mpu;	
	
	struct vector accel; // no packing needed 3 x 8
	struct vector gyro;
	struct vector rot;
	
	struct vector mag;
	double temperature_mag;
	
	struct vector global_accel;
	struct vector pos;

	/*-----------gps------------*/
	float longitude;
	float latitude;
	
	int32_t altitude;
	uint32_t time;
	
	uint32_t hdop;
	int32_t mag_head; //heading
	
	uint32_t count;
	uint32_t crc;
"""

class listenUDP(QThread):
	newMsg = pyqtSignal(dict)
	
	master = PktParser(dataset)
	slave = PktParser(dataset, master.end)
	fusion = PktParser(dataset, slave.end)
	
	dumpFile = open("logs/dump_{}.bin".format(datetime.datetime.now().strftime('%m%d-%H%M%S')), "bw")
	
	def __init__(self, port = 1234):
		QThread.__init__(self)
		self._udpServer = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
		self._udpServer.bind(("", port))
		self.port = port
		
	def run(self):
		while self._udpServer:
			try:
				data, addr = self._udpServer.recvfrom(2048)
				
			except WindowsError as e:
				print(e)
				time.sleep(1)
				continue
			if data == None:
					continue
		
			self.dumpFile.write(data + bytes("\r\n", 'ascii'))
			
			data_master = self.master.parse(data)
			data_slave  = self.slave.parse(data)
			data_fusion = self.fusion.parse(data)
			
			data = data_master

			if not data_master:
				continue
				
			for prop in self.master.keys():
				if data_slave:
					data["slave_" + prop] = data_slave[prop]
				if data_fusion:
					data["fusion_" + prop] = data_fusion[prop]
			
			data["datetime"] = str(datetime.datetime.now())
			
			if __name__ == "__main__":
				pp.pprint(data)
				
			self.newMsg.emit(data)
			time.sleep(0.01)
	
	def stop(self):
		self._udpServer.close()
		self._udpServer = None
		self.quit()
		
		if __name__ == "__main__":
			pp.pprint(obj)
			
		return

if __name__ == "__main__":
	udp = listenUDP(port = 1234)
	print("UDP Server is listening on port " + str(udp.port))
	udp.run()