diff --git a/rx/TelemetryGUI.py b/rx/TelemetryGUI.py index 40a7bfb..ee165f9 100644 --- a/rx/TelemetryGUI.py +++ b/rx/TelemetryGUI.py @@ -91,7 +91,7 @@ def updateGpsFrame(gps_data): # IMU Telemetry Data Frame # imuFrame = QtGui.QFrame() -imuFrame.setFixedSize(200,250) +imuFrame.setFixedSize(250,250) imuFrame.setFrameStyle(QtGui.QFrame.Box) imuFrame.setLineWidth(2) @@ -102,13 +102,13 @@ imuTemp = QtGui.QLabel("Temp:") imuTempValue = QtGui.QLabel("0 deg C") imuCalStatus = QtGui.QLabel("Calibration Status") imuCalStatus_System = QtGui.QLabel("System") -imuCalStatus_System_OK = QtGui.QLabel("BAD") +imuCalStatus_System_OK = QtGui.QLabel("?") imuCalStatus_Gyro = QtGui.QLabel("Gyro") -imuCalStatus_Gyro_OK = QtGui.QLabel("BAD") +imuCalStatus_Gyro_OK = QtGui.QLabel("?") imuCalStatus_Accel = QtGui.QLabel("Accel") -imuCalStatus_Accel_OK =QtGui.QLabel("BAD") +imuCalStatus_Accel_OK =QtGui.QLabel("?") imuCalStatus_Magnet = QtGui.QLabel("Mag") -imuCalStatus_Magnet_OK = QtGui.QLabel("BAD") +imuCalStatus_Magnet_OK = QtGui.QLabel("?") imuEulerLabel = QtGui.QLabel("Euler Angles") imuEulerHeadingLabel = QtGui.QLabel("Heading") imuEulerHeadingValue = QtGui.QLabel("0 deg") @@ -146,10 +146,12 @@ imuFrame.setLayout(imuFrameLayout) # IMU Plots. # -heading_values = np.array([0]) -roll_values = np.array([0]) -pitch_values = np.array([0]) -imu_times = np.array([0]) +buffer_size = 60 + +heading_values =[0] +roll_values = [0] +pitch_values = [0] +imu_times = [0] imuPlot = pg.GraphicsLayoutWidget() imuPlot_Heading = imuPlot.addPlot(title='Heading') @@ -160,26 +162,69 @@ imuPlotLayout.rowMaximumHeight(220) # Configure Plots imuPlot_Heading.setXRange(-1*imu_plot_history_size,0) -imuPlot_Heading.setYRange(-180,180) +imuPlot_Heading.setYRange(0,360) imuPlot_Roll.setXRange(-1*imu_plot_history_size,0) imuPlot_Roll.setYRange(-180,180) imuPlot_Pitch.setXRange(-1*imu_plot_history_size,0) imuPlot_Pitch.setYRange(-180,180) # Get curve objects so we can update them with new data. -heading_curve = imuPlot_Heading.plot(x=imu_times, y=heading_values) -roll_curve = imuPlot_Roll.plot(x=imu_times, y=roll_values) -pitch_curve = imuPlot_Pitch.plot(x=imu_times, y=pitch_values) +heading_curve = imuPlot_Heading.plot(x=imu_times, y=heading_values, pen=pg.mkPen('k',width=2)) +roll_curve = imuPlot_Roll.plot(x=imu_times, y=roll_values, pen=pg.mkPen('k',width=2)) +pitch_curve = imuPlot_Pitch.plot(x=imu_times, y=pitch_values, pen=pg.mkPen('k',width=2)) def updateIMUFrame(imu_data): """ Update IMU Frame and Plots with new IMU data """ - # To be Implemented - pass + global imu_times, heading_values, roll_values, pitch_values, heading_curve, roll_curve, pitch_curve + try: + imuTimeValue.setText(imu_data['timestamp']) + imuTempValue.setText("%d" % imu_data['temp']) + imuCalStatus_System_OK.setText("%d" % imu_data['sys_cal']) + imuCalStatus_Gyro_OK.setText("%d" % imu_data['gyro_cal']) + imuCalStatus_Accel_OK.setText("%d" % imu_data['accel_cal']) + imuCalStatus_Magnet_OK.setText("%d" % imu_data['magnet_cal']) + imuEulerHeadingValue.setText("%.1f" % imu_data['euler_heading']) + imuEulerRollValue.setText("%.1f" % imu_data['euler_roll']) + imuEulerPitchValue.setText("%.1f" % imu_data['euler_pitch']) + + latest_imu_time = imu_data['iTOW'] + latest_euler_heading = imu_data['euler_heading'] + latest_euler_roll = imu_data['euler_roll'] + latest_euler_pitch = imu_data['euler_pitch'] + + if imu_times[0] == 0: + # We need to initialise the arrays. + imu_times[0] = latest_imu_time + heading_values[0] = latest_euler_heading + roll_values[0] = latest_euler_roll + pitch_values[0] = latest_euler_pitch + else: + # Append, and crop arrays if they are too big. + imu_times.append(latest_imu_time) + heading_values.append(latest_euler_heading) + roll_values.append(latest_euler_roll) + pitch_values.append(latest_euler_pitch) + + if len(imu_times) > buffer_size: + imu_times = imu_times[-1*buffer_size:] + heading_values = heading_values[-1*buffer_size:] + roll_values = roll_values[-1*buffer_size:] + pitch_values = pitch_values[-1*buffer_size:] + + # Update plots + heading_curve.setData(x=(np.array(imu_times)-latest_imu_time), y=heading_values) + roll_curve.setData(x=(np.array(imu_times)-latest_imu_time), y = roll_values) + pitch_curve.setData(x=(np.array(imu_times)-latest_imu_time), y=pitch_values) + + + except: + traceback.print_exc() + pass # Telemetry Log packetSnifferFrame = QtGui.QFrame() -packetSnifferFrame.setFixedSize(1200,150) +packetSnifferFrame.setFixedSize(1300,150) packetSnifferFrame.setFrameStyle(QtGui.QFrame.Box) packetSnifferTitle = QtGui.QLabel("Telemetry Log") console = QtGui.QPlainTextEdit() @@ -203,7 +248,7 @@ layout.addWidget(packetSnifferFrame,1,0,1,4) mainwin = QtGui.QMainWindow() mainwin.setWindowTitle("Wenet GPS/IMU Telemetry Console") mainwin.setCentralWidget(main_widget) -mainwin.resize(1200,400) +mainwin.resize(1300,400) mainwin.show() @@ -221,20 +266,18 @@ def process_udp(udp_packet): packet_dict = json.loads(udp_packet) packet = packet_dict['packet'] - # Convert to string, and print to terminal with timestamp. - console.appendPlainText("%s \t%s" % (timestamp,packet_to_string(packet))) - # Decode GPS and IMU packets, and pass onto their respective GUI update functions. packet_type = decode_packet_type(packet) if packet_type == WENET_PACKET_TYPES.GPS_TELEMETRY: gps_data = gps_telemetry_decoder(packet) updateGpsFrame(gps_data) - elif packet_type == WENET_PACKET_TYPES.IMU_TELEMETRY: - imu_data = imu_telemetry_decoder(packet) - updateIMUFrame(imu_data) + elif packet_type == WENET_PACKET_TYPES.ORIENTATION_TELEMETRY: + orientation_data = orientation_telemetry_decoder(packet) + updateIMUFrame(orientation_data) else: - pass + # Convert to string, and print to terminal with timestamp. + console.appendPlainText("%s \t%s" % (timestamp,packet_to_string(packet))) udp_listener_running = False