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

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

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

from vispy import app, visuals, scene

from main_ui import Ui_MainWindow
from settings_ui import Ui_Dialog as Ui_Settings

import numpy as np
import random
from colorsys import hsv_to_rgb
import pprint
import datetime, time
from collections import deque

from functions import *
from listenUDP import listenUDP
from pingThread import pingThread
import vispyHelper

import csv
import socket


random.seed()

class AppWindow(QMainWindow):
	
	csv = None
	rate = 10
	
	vel = [0] * 3
	vel2 = [0] * 3
	pos = [0] * 3
	pos2 = [0] * 3
	
	oldSlaveCount = 0
	
	def __init__(self):
		super().__init__()
		self.ui = Ui_MainWindow()
		self.ui.setupUi(self)
		
		self.settings = QSettings("bexus", "imufusion")
		
		self.master_rate = [0] * self.rate
		self.slave_rate  = [0] * self.rate
		
		self.ip = getSettings(self.settings, 'ip', "172.16.18.171")
		self.port = getSettings(self.settings, 'port', 1234)
		logFile = getSettings(self.settings, 'logFile', "default")
		self.logFile = open("logs/{}_{}.txt".format(logFile, datetime.datetime.now().strftime('%m%d-%H%M%S')), "w", newline='')
		
		
		self.ui.labelAddress.setText("{}:{}".format(self.ip, self.port))
		self.ui.pushButton_reset.clicked.connect(self.reset_pressed)
		
		self.udpServer = listenUDP(self.port)
		self.udpServer.newMsg.connect(self.onNewMsg)
		
		self.ui.tableWidget.setColumnCount(3)
		self.ui.tableWidget.setHorizontalHeaderLabels(("Master", "Slave", "Fusion"))
		
		self.fields = self.udpServer.master.keys()
		self.ui.tableWidget.setRowCount(len(self.fields))
		self.ui.tableWidget.setVerticalHeaderLabels(self.fields)
		
		self.pingThread = pingThread(self.ip)
		self.pingThread.pong.connect(self.onPong)
		
		self.data3d = np.array([[0,0,0]], np.float32)
		self.data3d2 = np.array([[0,0,0]], np.float32)
		
		self.view3D = vispyHelper.view3D(self.ui.openGLWidget)
		self.viewQuad = vispyHelper.viewQuad(self.ui.openGLWidgetQuad)

		
		self.udpServer.start()
		self.pingThread.start()
		
		self.timer = app.Timer()
		self.timer.connect(self.update)
		self.timer.start(0.1)
		
		self.show()
		
	def onNewMsg(self, data):
	
		### MAIN STUFF ###
		
		#self.ui.labelTestData.setText(pprint.pformat(data))
		
		if self.csv == None:
			self.csv = csv.DictWriter(self.logFile, data.keys())
			self.csv.writeheader()
			
		self.csv.writerow(data)
		
		self.master_rate.append(time.time() * 1000)
		self.master_rate.pop(0)
		
		if data["slave_count"] != self.oldSlaveCount:
			self.slave_rate.append(time.time() * 1000)
			self.slave_rate.pop(0)
			
		self.oldSlaveCount = data["slave_count"];
		
		delayMaster = (max(self.master_rate) - min(self.master_rate)) / self.rate
		delaySlave = (max(self.slave_rate) - min(self.slave_rate)) / self.rate 
		
		self.ui.labelLink_Master.setText("{:.1f}/s".format(1000 / delayMaster if delayMaster!=0 else 0))
		self.ui.labelLink_Slave.setText("{:.1f}/s".format(1000 / delaySlave if delaySlave!=0 else 0))
		
		
		rx, ry, rz =data["rot"]
		heading = data["mag_head"]
		
		if rz != 0:
			rx = np.arctan(ry / rz)
			ry = np.arctan(rx / rz)
		else:
			rx = np.arctan(0)
			ry = np.arctan(0)
		rz = 180 + heading
		
		rx *= 45
		ry *= -45
		
		rx += 90
		rz -= 90
		
		#self.data3d = np.append(self.data3d, [data["pos"]], axis=0)
		#self.data3d = np.append(self.data3d, [[data["latitude"], data["longitude"], data["altitude"]]], axis=0)
		
		self.vel[0] += data["global_accel"][0]
		self.vel[1] += data["global_accel"][1]
		self.vel[2] += data["global_accel"][2]
		
		self.pos[0] += self.vel[0]
		self.pos[1] += self.vel[1]
		self.pos[2] += self.vel[2] - 2
		
		self.vel2[0] += data["slave_global_accel"][0]
		self.vel2[1] += data["slave_global_accel"][1]
		self.vel2[2] += data["slave_global_accel"][2]
		
		self.pos2[0] += self.vel2[0]
		self.pos2[1] += self.vel2[1]
		self.pos2[2] += self.vel2[2] - 2
		
		self.data3d = np.append(self.data3d, [self.pos], axis=0)
		self.data3d2 = np.append(self.data3d2, [self.pos], axis=0)
		
		if self.ui.tabWidget.currentIndex() == 0:
			self.view3D.update((self.data3d, self.data3d), (rx, ry, rz))
		elif self.ui.tabWidget.currentIndex() == 1:
			self.viewQuad.update((self.data3d, self.data3d2), (rx, ry, rz))
		else:
			i = 0
			for k in self.fields:
				self.ui.tableWidget.setItem(i,0, QTableWidgetItem(str(data[k])))
				self.ui.tableWidget.setItem(i,1, QTableWidgetItem(str(data["slave_"+k])))
				self.ui.tableWidget.setItem(i,2, QTableWidgetItem(str(data["fusion_"+k])))
				
				i+=1
				
				
		self.ui.label_temp_adc_1.setText("{:,.2g} °C".format(data["adc_temperature"]))
		self.ui.label_temp_cis_1.setText("{:.2g} °C".format(data["temperature_cis"]))
		self.ui.label_temp_mag_1.setText("{:.2g} °C".format(data["temperature_mag"]))
		self.ui.label_temp_mpu_1.setText("{:.2g} °C".format(data["temperature_mpu"]))
		
		self.updateVector("label_Ac", data["accel"])
		self.updateVector("label_Gy", data["gyro"])
		self.updateVector("label_Ma", data["mag"])
		self.updateVector("label_Ga", data["global_accel"])
		self.updateVector("label_Rot", data["rot"])
		self.updateVector("label_Pos", self.pos)
		
		self.ui.label_pres_cis_1.setText("{:f} Pa".format(data["pressure_cis"]))
		self.ui.label_pres_i2c_1.setText("{:f} Pa".format(data["pressure_spi"]))	
		
		self.ui.label_lat_1.setText("{:.5g}".format(data["latitude"]))
		self.ui.label_lon_1.setText("{:.5g}".format(data["longitude"]))
		self.ui.label_alt_1.setText("{:.2f} m".format(data["altitude"]))
		
		##--------------------------------------------------------------------------------##	
		self.ui.label_temp_mp.setText("{:g} °C".format(data["adc_temperature_internal"]))
		self.ui.label_temp_i2c.setText("{:g} °C".format(data["temperature_i2c"]))
		
		self.ui.label_voltage.setText("{:.3f} V".format(data["adc_voltage"] * 1.0603))		
		self.ui.label_current.setText("{:.2f} mA".format(data["adc_current"] * 1000))	
		
		##--------------------------------------------------------------------------------##	
	
		try:
			dt = datetime.datetime.fromtimestamp(data["time"] / 1000)
			#self.ui.label_date_1.setText(dt.strftime('%d.%m.%Y'))
			self.ui.label_time_1.setText(dt.strftime('%H:%M:%S'))
		except(OSError):
			pass
		self.ui.label_hdop_1.setText("{:g}".format(data["hdop"]))
	
	def updateVector(self, elem, data, f = "{:g}"):
		getattr(self.ui, elem+"X").setText(f.format(data[0]))
		getattr(self.ui, elem+"Y").setText(f.format(data[1]))
		getattr(self.ui, elem+"Z").setText(f.format(data[2]))
	
	def update(self, ev):
		#reset
		value = self.ui.progressBar_reset.value()
		if value > 0:
			value -= 1
			self.ui.progressBar_reset.setValue(value)
			
		return
	
	def reset_pressed(self):
		value = self.ui.progressBar_reset.value()
		value += 10
		if value > 100:
			value = 0
			sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
			sock.sendto(bytes("RESET!!", "utf-8"), (self.ip, 65100))
			
		self.ui.progressBar_reset.setValue(value)
	
	def onPong(self, t):
		if t != -1:
			self.ui.labelLatency.setText("{:.1f}ms".format(t*1000))
			self.ui.labelLink.setText("Online")
			self.ui.labelLink.setStyleSheet("color:white;background-color:green")
		else:
			self.ui.labelLatency.setText("")
			self.ui.labelLink.setText("Offline")
			self.ui.labelLink.setStyleSheet("color:white;background-color:red;")

	@pyqtSlot(bool)
	def on_actionSettings_triggered(self, triggered):
		self.settings

		d = QDialog()
		d.ui = Ui_Settings()
		d.ui.setupUi(d)
		d.ui.lineEditIP.setText(self.ip)
		d.ui.lineEditPort.setText(str(self.port))
		
		if d.exec_():
			self.ip = d.ui.lineEditIP.text()
			self.port = safe_cast(d.ui.lineEditPort.text(), int, 1234)
			logFile = d.ui.lineEditLogFile.text()
			self.settings.setValue('ip', self.ip)
			self.settings.setValue('port', self.port)
			self.settings.setValue('logFile', logFile)
			
			self.logFile = open("logs/{}_{}.txt".format(logFile, datetime.datetime.now().strftime('%m%d-%H%M%S')), "w")
			self.csv = None
			
			self.pingThread.changeIP(self.ip)
			self.udpServer.stop()
			self.udpServer = listenUDP(self.port)
			self.udpServer.newMsg.connect(self.onNewMsg)
			self.udpServer.start()
			
			self.ui.labelAddress.setText("{}:{}".format(self.ip, self.port))

			# this writes the settings to storage
			self.settings.sync()
	
	def on_actionFollow_triggered(self):
		pass