caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Working readout for beacon frequency thanks to Dylan!

Browse Source
This commit is contained in:
Corwin Perren
2018-08-09 22:21:18 -07:00
parent d9e8458797
commit c3a98cc0a7
6 changed files with 293 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py
View File

@@ -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)

61
software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py
View File

@@ -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)

198
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -1390,15 +1390,98 @@ N/A</string>
</size>
</property>
<property name="currentIndex">
<number>3</number>
<number>0</number>
</property>
<widget class="QWidget" name="tab_5">
<attribute name="title">
<string>RDF</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_16">
<item row="1" column="0">
<spacer name="verticalSpacer_6">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>111</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="0">
<widget class="QLCDNumber" name="rssi_lcdnumber"/>
<layout class="QVBoxLayout" name="verticalLayout_20">
<item>
<layout class="QVBoxLayout" name="verticalLayout_21">
<item>
<widget class="QLabel" name="label_31">
<property name="font">
<font>
<pointsize>16</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Raw Data</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="rssi_lcdnumber">
<property name="minimumSize">
<size>
<width>400</width>
<height>150</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_22">
<item>
<widget class="QLabel" name="label_32">
<property name="font">
<font>
<pointsize>16</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Beacon Frequency</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="beacon_frequency_lcd_number">
<property name="minimumSize">
<size>
<width>400</width>
<height>150</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="1">
<spacer name="horizontalSpacer_19">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
@@ -2206,7 +2289,9 @@ Permittivity</string>
</widget>
</item>
<item>
<widget class="QLabel" name="label_23">
<layout class="QHBoxLayout" name="horizontalLayout_19">
<item>
<widget class="QLabel" name="label_27">
<property name="font">
<font>
<weight>75</weight>
@@ -2218,6 +2303,67 @@ Permittivity</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_18">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="ssh_scan_for_hosts_button">
<property name="text">
<string>Scan For Hosts</string>
</property>
</widget>
</item>
<item>
<widget class="Line" name="line_11">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_23">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Host</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="ssh_host_line_edit">
<property name="minimumSize">
<size>
<width>125</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>125</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>task.cstag.ca</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_18">
<item row="0" column="0">
@@ -2749,29 +2895,6 @@ Permittivity</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="manual_waypoint_degrees_longitude_spin_box">
<property name="suffix">
<string>° </string>
</property>
<property name="minimum">
<number>-180</number>
</property>
<property name="maximum">
<number>180</number>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QSpinBox" name="manual_waypoint_minutes_longitude_spin_box">
<property name="suffix">
<string>'</string>
</property>
<property name="maximum">
<number>60</number>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QComboBox" name="manual_waypoint_cardinal_lattitude_combo_box">
<item>
@@ -2791,6 +2914,19 @@ Permittivity</string>
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="manual_waypoint_degrees_longitude_spin_box">
<property name="suffix">
<string>° </string>
</property>
<property name="minimum">
<number>-180</number>
</property>
<property name="maximum">
<number>180</number>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QComboBox" name="manual_waypoint_cardinal_longitude_combo_box">
<item>
@@ -2810,6 +2946,16 @@ Permittivity</string>
</item>
</widget>
</item>
<item row="1" column="2">
<widget class="QSpinBox" name="manual_waypoint_minutes_longitude_spin_box">
<property name="suffix">
<string>'</string>
</property>
<property name="maximum">
<number>60</number>
</property>
</widget>
</item>
</layout>
</item>
<item>

128
software/ros_packages/rover_science/src/freq_estimates.py
View File

@@ -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)

18
software/scripts/equipment_servicing_interface.py
View File

@@ -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

30
software/testing/rdf_data_processing.py Executable file
View File

@@ -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()