From c3a98cc0a7f6d5a7712e7a6f16956a82ca9bfa14 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 9 Aug 2018 22:21:18 -0700 Subject: [PATCH] Working readout for beacon frequency thanks to Dylan! --- .../Framework/MiscSystems/BashConsoleCore.py | 40 +++- .../src/Framework/MiscSystems/RDFCore.py | 61 ++++- .../src/Resources/Ui/left_screen.ui | 218 +++++++++++++++--- .../rover_science/src/freq_estimates.py | 128 ---------- .../scripts/equipment_servicing_interface.py | 18 -- software/testing/rdf_data_processing.py | 30 +++ 6 files changed, 293 insertions(+), 202 deletions(-) delete mode 100644 software/ros_packages/rover_science/src/freq_estimates.py create mode 100755 software/testing/rdf_data_processing.py diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py index c21efff..c63eea8 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py @@ -14,9 +14,9 @@ import paramiko ##################################### THREAD_HERTZ = 5 -IP = "192.168.1.10" -USER = "nvidia" -PASS = "nvidia" +IP = "192.168.1.127" +USER = "caperren" +PASS = "ult1m2t3!" ##################################### @@ -36,6 +36,9 @@ class BashConsole(QtCore.QThread): self.console_text_edit = self.left_screen.console_line_edit # type: QtWidgets.QTextEdit self.ssh_console_command_line_edit = self.left_screen.ssh_console_command_line_edit # type:QtWidgets.QLineEdit + self.ssh_scan_for_hosts_button = self.left_screen.ssh_scan_for_hosts_button # type: QtWidgets.QPushButton + self.ssh_host_line_edit = self.left_screen.ssh_host_line_edit # type: QtWidgets.QLineEdit + self.ssh_list_wifi_button = self.left_screen.ssh_list_wifi_button # type: QtWidgets.QPushButton self.ssh_equipment_login_button = self.left_screen.ssh_equipment_login_button # type: QtWidgets.QPushButton self.ssh_equipment_logout_button = self.left_screen.ssh_equipment_logout_button # type: QtWidgets.QPushButton @@ -118,25 +121,44 @@ class BashConsole(QtCore.QThread): self.new_command = True def on_login_button_pressed__slot(self): - self.new_command_text = "python equipment_servicing_interface.py 'task.cstag.ca' 'LOGIN MTECH GITRDONE' HELP" + current_ip = self.ssh_host_line_edit.text() + self.new_command_text = "python equipment_servicing_interface.py '%s' 'LOGIN MTECH GITRDONE' HELP" % current_ip + print self.new_command_text self.new_command = True def on_logout_button_pressed__slot(self): - self.new_command_text = "python equipment_servicing_interface.py 'task.cstag.ca' LOGOUT" + current_ip = self.ssh_host_line_edit.text() + self.new_command_text = "python equipment_servicing_interface.py '%s' LOGOUT" % current_ip self.new_command = True def on_status_button_pressed__slot(self): - self.new_command_text = "python equipment_servicing_interface.py 'task.cstag.ca' STATUS" + current_ip = self.ssh_host_line_edit.text() + self.new_command_text = "python equipment_servicing_interface.py '%s' STATUS" % current_ip self.new_command = True def on_start_button_pressed__slot(self): - self.new_command_text = "python equipment_servicing_interface.py 'task.cstag.ca' START" + current_ip = self.ssh_host_line_edit.text() + self.new_command_text = "python equipment_servicing_interface.py '%s' START" % current_ip self.new_command = True def on_stop_button_pressed__slot(self): - self.new_command_text = "python equipment_servicing_interface.py 'task.cstag.ca' STOP" + current_ip = self.ssh_host_line_edit.text() + self.new_command_text = "python equipment_servicing_interface.py '%s' STOP" % current_ip self.new_command = True + def on_ssh_scan_for_hosts_pressed__slot(self): + current_ip = self.ssh_host_line_edit.text() + + find_dot = current_ip.rfind(".") + + if find_dot > 0: + current_ip = current_ip[:find_dot + 1] + "0" + self.new_command_text = "nmap -sP %s/24 -oG - | awk '/Up$/{print $2}'" % current_ip + self.new_command = True + else: + self.set_text_contents += "IP address for range search not valid. Try again." + self.text_update_ready__signal.emit(self.set_text_contents) + def on_connect_ssid_button_pressed__slot(self): ssid_text = self.ssh_ssid_line_edit.text() @@ -156,6 +178,8 @@ class BashConsole(QtCore.QThread): self.ssh_console_command_line_edit.editingFinished.connect(self.on_text_editing_finished__slot) self.console_text_edit.textChanged.connect(self.on_text_readout_updated__slot) + self.ssh_scan_for_hosts_button.clicked.connect(self.on_ssh_scan_for_hosts_pressed__slot) + self.ssh_equipment_login_button.clicked.connect(self.on_login_button_pressed__slot) self.ssh_equipment_logout_button.clicked.connect(self.on_logout_button_pressed__slot) self.ssh_equipment_status_button.clicked.connect(self.on_status_button_pressed__slot) diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py index b7e128b..9fbd0a4 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py @@ -7,6 +7,8 @@ from PyQt5 import QtCore, QtWidgets import logging import rospy from time import time +import scipy.fftpack +import numpy from std_msgs.msg import Float64MultiArray @@ -23,7 +25,8 @@ THREAD_HERTZ = 5 ##################################### class RDF(QtCore.QThread): - lcd_number_update_ready__signal = QtCore.pyqtSignal(int) + rssi_lcd_number_update_ready__signal = QtCore.pyqtSignal(int) + beacon_lcd_number_update_ready__signal = QtCore.pyqtSignal(float) def __init__(self, shared_objects): super(RDF, self).__init__() @@ -33,6 +36,7 @@ class RDF(QtCore.QThread): self.left_screen = self.shared_objects["screens"]["left_screen"] self.rssi_lcdnumber = self.left_screen.rssi_lcdnumber # type:QtWidgets.QLCDNumber + self.beacon_frequency_lcd_number = self.left_screen.beacon_frequency_lcd_number # type:QtWidgets.QLCDNumber # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -48,9 +52,11 @@ class RDF(QtCore.QThread): self.rdf_subscriber = rospy.Subscriber(RDF_DATA_TOPIC, Float64MultiArray, self.new_rdf_message_received__callback) - self.data = [] - self.data_limit = 100 + self.moving_average_raw_data = [] + self.raw_data = numpy.array([]) + self.raw_data_timestamps = numpy.array([]) + self.data_window_size = 200 def run(self): self.logger.debug("Starting RDF Thread") @@ -58,10 +64,40 @@ class RDF(QtCore.QThread): while self.run_thread_flag: start_time = time() - temp = list(self.data) + temp = list(self.moving_average_raw_data) if temp: average = sum(temp) / len(temp) - self.lcd_number_update_ready__signal.emit(average) + self.rssi_lcd_number_update_ready__signal.emit(average) + + if self.raw_data.size >= self.data_window_size and self.raw_data_timestamps.size >= self.data_window_size: + + try: + time_step_array = numpy.array([]) + for n in range(0, self.data_window_size - 1): + time_step_array = numpy.append(time_step_array, self.raw_data_timestamps[n + 1] - self.raw_data_timestamps[n]) + + T = numpy.average(time_step_array) + + yf = scipy.fftpack.fft(self.raw_data) + xf = numpy.linspace(0.0, 1.0 / (2.0 * T), self.data_window_size / 2) + + valid_range = [] + + for n in range(0, len(xf)): + if (xf[n] > 0.5) and (xf[n] < 5.0): + valid_range.append(n) + + yf = numpy.take(yf, valid_range) + xf = numpy.take(xf, valid_range) + + max_index = numpy.argmax(numpy.abs(yf)) + freq = xf[max_index] + self.beacon_lcd_number_update_ready__signal.emit(freq) + except: + pass + + self.raw_data = numpy.array([]) + self.raw_data_timestamps = numpy.array([]) time_diff = time() - start_time @@ -70,17 +106,18 @@ class RDF(QtCore.QThread): self.logger.debug("Stopping RDF Thread") def new_rdf_message_received__callback(self, data): - if len(self.data) >= self.data_limit: - del self.data[0] + if len(self.moving_average_raw_data) >= self.data_window_size: + del self.moving_average_raw_data[0] - # if len(self.times) >= self.data_limit: - # del self.times[0] + self.moving_average_raw_data.append(data.data[0]) - self.data.append(data.data[0]) - # self.times.append(data.data[1]) + if self.raw_data.size != self.data_window_size and self.raw_data_timestamps.size != self.data_window_size: + self.raw_data = numpy.append(self.raw_data, data.data[0]) + self.raw_data_timestamps = numpy.append(self.raw_data_timestamps, data.data[1]) def connect_signals_and_slots(self): - self.lcd_number_update_ready__signal.connect(self.rssi_lcdnumber.display) + self.rssi_lcd_number_update_ready__signal.connect(self.rssi_lcdnumber.display) + self.beacon_lcd_number_update_ready__signal.connect(self.beacon_frequency_lcd_number.display) def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): start_signal.connect(self.start) diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 000069b..9bd74e6 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1390,15 +1390,98 @@ N/A - 3 + 0 RDF + + + + Qt::Vertical + + + + 20 + 111 + + + + - + + + + + + + + 16 + 75 + true + + + + Raw Data + + + + + + + + 400 + 150 + + + + + + + + + + + + + 16 + 75 + true + + + + Beacon Frequency + + + + + + + + 400 + 150 + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + @@ -2206,17 +2289,80 @@ Permittivity - - - - 75 - true - - - - Preset Commands - - + + + + + + 75 + true + + + + Preset Commands + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Scan For Hosts + + + + + + + Qt::Vertical + + + + + + + + 75 + true + + + + Host + + + + + + + + 125 + 0 + + + + + 125 + 16777215 + + + + task.cstag.ca + + + + @@ -2749,29 +2895,6 @@ Permittivity - - - - ° - - - -180 - - - 180 - - - - - - - ' - - - 60 - - - @@ -2791,6 +2914,19 @@ Permittivity + + + + ° + + + -180 + + + 180 + + + @@ -2810,6 +2946,16 @@ Permittivity + + + + ' + + + 60 + + + diff --git a/software/ros_packages/rover_science/src/freq_estimates.py b/software/ros_packages/rover_science/src/freq_estimates.py deleted file mode 100644 index 8bc1604..0000000 --- a/software/ros_packages/rover_science/src/freq_estimates.py +++ /dev/null @@ -1,128 +0,0 @@ -from __future__ import division -from numpy.fft import rfft -from numpy import argmax, mean, diff, log -from matplotlib.mlab import find -from scipy.signal import blackmanharris, fftconvolve -from numpy import polyfit, arange - - -def freq_from_crossings(sig, fs): - """ - Estimate frequency by counting zero crossings - """ - # Find all indices right before a rising-edge zero crossing - indices = find((sig[1:] >= 0) & (sig[:-1] < 0)) - - # Naive (Measures 1000.185 Hz for 1000 Hz, for instance) - # crossings = indices - - # More accurate, using linear interpolation to find intersample - # zero-crossings (Measures 1000.000129 Hz for 1000 Hz, for instance) - crossings = [i - sig[i] / (sig[i+1] - sig[i]) for i in indices] - - # Some other interpolation based on neighboring points might be better. - # Spline, cubic, whatever - - return fs / mean(diff(crossings)) - - -def freq_from_fft(sig, fs): - """ - Estimate frequency from peak of FFT - """ - # Compute Fourier transform of windowed signal - windowed = sig * blackmanharris(len(sig)) - f = rfft(windowed) - - # Find the peak and interpolate to get a more accurate peak - i = argmax(abs(f)) # Just use this for less-accurate, naive version - true_i = parabolic(log(abs(f)), i)[0] - - # Convert to equivalent frequency - return fs * true_i / len(windowed) - - -def freq_from_autocorr(sig, fs): - """ - Estimate frequency using autocorrelation - """ - # Calculate autocorrelation (same thing as convolution, but with - # one input reversed in time), and throw away the negative lags - corr = fftconvolve(sig, sig[::-1], mode='full') - corr = corr[len(corr)//2:] - - # Find the first low point - d = diff(corr) - start = find(d > 0)[0] - - # Find the next peak after the low point (other than 0 lag). This bit is - # not reliable for long signals, due to the desired peak occurring between - # samples, and other peaks appearing higher. - # Should use a weighting function to de-emphasize the peaks at longer lags. - peak = argmax(corr[start:]) + start - px, py = parabolic(corr, peak) - - return fs / px - - -def freq_from_HPS(sig, fs): - """ - Estimate frequency using harmonic product spectrum (HPS) - """ - windowed = sig * blackmanharris(len(sig)) - - from pylab import subplot, plot, log, copy, show - - # harmonic product spectrum: - c = abs(rfft(windowed)) - maxharms = 8 - subplot(maxharms, 1, 1) - plot(log(c)) - for x in range(2, maxharms): - a = copy(c[::x]) # Should average or maximum instead of decimating - # max(c[::x],c[1::x],c[2::x],...) - c = c[:len(a)] - i = argmax(abs(c)) - true_i = parabolic(abs(c), i)[0] - print 'Pass %d: %f Hz' % (x, fs * true_i / len(windowed)) - c *= a - subplot(maxharms, 1, x) - plot(log(c)) - show() - - -def parabolic(f, x): - """Quadratic interpolation for estimating the true position of an - inter-sample maximum when nearby samples are known. - - f is a vector and x is an index for that vector. - - Returns (vx, vy), the coordinates of the vertex of a parabola that goes - through point x and its two neighbors. - - Example: - Defining a vector f with a local maximum at index 3 (= 6), find local - maximum if points 2, 3, and 4 actually defined a parabola. - - In [3]: f = [2, 3, 1, 6, 4, 2, 3, 1] - - In [4]: parabolic(f, argmax(f)) - Out[4]: (3.2142857142857144, 6.1607142857142856) - - """ - xv = 1 / 2. * (f[x - 1] - f[x + 1]) / (f[x - 1] - 2 * f[x] + f[x + 1]) + x - yv = f[x] - 1 / 4. * (f[x - 1] - f[x + 1]) * (xv - x) - return (xv, yv) - - -def parabolic_polyfit(f, x, n): - """Use the built-in polyfit() function to find the peak of a parabola - - f is a vector and x is an index for that vector. - - n is the number of samples of the curve used to fit the parabola. - """ - a, b, c = polyfit(arange(x - n // 2, x + n // 2 + 1), f[x - n // 2:x + n // 2 + 1], 2) - xv = -0.5 * b / a - yv = a * xv ** 2 + b * xv + c - return (xv, yv) diff --git a/software/scripts/equipment_servicing_interface.py b/software/scripts/equipment_servicing_interface.py index c021a42..1f42ab7 100644 --- a/software/scripts/equipment_servicing_interface.py +++ b/software/scripts/equipment_servicing_interface.py @@ -20,21 +20,3 @@ for command in commands: data, server = sock.recvfrom(4096) print data - -# while True: -# try: -# # Send data -# message = raw_input() -# # print type(message) -# if message not in messages: -# print "Invalid command. Please try again." -# continue -# -# sent = sock.sendto(message, server_address) -# -# # Receive response -# # print 'waiting to receive' -# data, server = sock.recvfrom(4096) -# print data -# except: -# pass \ No newline at end of file diff --git a/software/testing/rdf_data_processing.py b/software/testing/rdf_data_processing.py new file mode 100755 index 0000000..3dae98d --- /dev/null +++ b/software/testing/rdf_data_processing.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python +import rospy +import time +from std_msgs.msg import Float64MultiArray + +class RDFProceesor(object): + def __init__(self): + super(RDFProceesor, self).__init__() + self.sub = rospy.Subscriber("/rover_science/rdf/data", Float64MultiArray, self.on_rdf_data_received) + self.new_data = False + self.current_data = None + + print self.sub + + def run(self): + while True: + if self.new_data: + print self.current_data + self.new_data = False + time.sleep(0.01) + + def on_rdf_data_received(self, rdf_data): + print rdf_data + self.current_data = rdf_data + self.new_data = True + + +if __name__ == '__main__': + processor = RDFProceesor() + processor.run() \ No newline at end of file