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

import socket
import time
import pprint
import argparse
import sys, os

sys.path.insert(0,'..')

from functions import *

import numpy as np
import matplotlib.pyplot as plt

pp = pprint.PrettyPrinter(indent=4)

parser = argparse.ArgumentParser()
parser.add_argument("input_file")
args = parser.parse_args()

size = os.stat(args.input_file).st_size
print("file name: {} size: {} bytes".format(args.input_file, size))
f = open(args.input_file, "rb")

#fw = open("data.csv", "w")

buf = b''
length = 1024

t = []
	
result = {
	"mag_x": [],
	"temperature_mpu": [],
	#"adc_temperature": [],
}
axis = [0, 1, 1]
count = 0
colors = ["tab:red", "tab:blue", "tab:green"]



dataset = """
/*-------RAW-----------*/
	uint32_t pressure_cis_raw;		// pack 1 x 4
	uint32_t temperature_cis_raw; //		+ 1 x 4
	
	uint32_t pressure_i2c_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
	
	/*-----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;

	/*-----------gps------------*/
	int32_t longitude;
	int32_t longitude_mod2;
	
	int32_t latitude;
	int32_t latitude_mod2;
	
	int32_t altitude;
	int32_t altitude_mod2;
	
	uint32_t time;
	uint32_t time_mod2;
	
	uint32_t hdop;
	uint32_t hdop_mod2;
	/*--------------------------*/	

	/*---------magnet-----------*/
	struct vector_Uint mag_raw;	// pack 3 x 4
	int32_t temperature_mag_raw; //		+ 1 x 4
	
	struct vector mag;

	double temperature_mag;
	
	uint32_t count;
	uint32_t crc;
"""

master = PktParser(dataset)
slave = PktParser(dataset, master.end)
fusion = PktParser(dataset, slave.end)
	
def check():
	global count
	#if buf[length-2] != ord("\r") or buf[length-1] != ord("\n"):
	if buf.find(b"\r\n") == -1:
		print("[{:8}] error missing EOL! bufLen={}".format(count, len(buf)))
		return
	
	obj = master.parse(buf)
	obj2 = slave.parse(buf)
	
	#if count == 0:
		#fw.write(",".join(obj.keys()) + "\n")
	
	if obj["count"] != 0: #obj2["count"]:
		print("[{:8}] count error! {} != {}".format(count, obj["count"], obj2["count"]))
		return
	if obj["crc"] != 1094795585: #obj2["crc"]:
		print("[{:8}] crc error! crc={}".format(count, obj["crc"]))
		return
	
	#if count != obj["count"] - 1:
	#	print("[{0:8}] missing data between row {0} and {1}!".format(count, obj["count"]))
	#count = obj["count"]
	count += 1
	
	t.append(count/10)
	for key in result:
		result[key].append(obj[key])
	
	if count % 10000 == 0:
		print("[{:8}] Reading...".format(count))
	
	return obj
try:
	byte = b'A'
	while byte != b"":
		# Do stuff with byte.
		if len(buf) >= length:
			obj = check()
			
			#if obj:
				#fw.write(",".join(str(x) for x in obj.values()) + "\n")
			
			
			pos = buf.find(b"\r\n")
			if pos == -1:
				print("[{:8}] can't find next line! bufLen={}".format(count, len(buf)))
			
			buf = buf[pos+2:]
		else:
			byte = f.read(length)
			buf += byte
			
	#if obj != None:
		
	#	pp.pprint(obj)
finally:
	f.close()
	#fw.close()

	fig, ax1 = plt.subplots()
	ax1.set_xlabel('time (s)')
	
	n = 0
	
	ax2 = ax1.twinx()
	axes = [ax1, ax2]
		
	for key in result:
		
		axes[axis[n]].plot(t, result[key], label=key, color=colors[n])
		axes[axis[n]].set_ylabel(key, color=colors[n])
		axes[axis[n]].tick_params(axis='y', labelcolor=colors[n])
		n += 1
		
	#plt.legend()
	fig.tight_layout()
	plt.show()