def __init__(self, parent=None):
QtWidgets.QMainWindow.__init__(self)
main_windows = QtWidgets.QMainWindow()
Ui_MiClassGui.setupUi(self, main_windows)
SerialCommunication.__init__(self)
self.setupUi(self)
self.dialog = QtWidgets.QDialog()
self.dialog.ui = Ui_dialog_port_conf()
self.dialog.ui.setupUiDialog(self.dialog)
# configuration buttons connection
self.btn_config_port.clicked.connect(self.dialog_config_port)
self.btn_quit.clicked.connect(self.close_application)
self.dialog.ui.btn_ok_cancel.accepted.connect(self.accept)
self.dialog.ui.btn_ok_cancel.rejected.connect(self.reject)
self.btn_start.clicked.connect(self.start_serial_communication)
self.btn_stop.clicked.connect(self.close_serial_communication)
self.btn_send.clicked.connect(self.send_text)
self.txt_send.returnPressed.connect(self.send_text)
self.btn_color.clicked.connect(self.get_color)
# private attribute to check if will be used the self configuration
self.__self_configuration = False
self.__filled_dialog = False
self.port_selected = ''
self.color = '00FF00'
# create timer (thread) execution
self.timer = QtCore.QTimer()
self.timer.setInterval(1000)
self.timer.timeout.connect(self.timer_process)
self.__fill_cmb_end_line()
def __init__(self, serialPort):
QThread.__init__(self)
self.serialPort = serialPort
self.serialComm = SerialCommunication(serialPort)
self.cmd = comm_packet_pb2.CommandPacket()
control_signal_cmd = self.cmd.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 0.0
class CommQThread(QThread):
def __init__(self, serialPort):
QThread.__init__(self)
self.serialPort = serialPort
self.serialComm = SerialCommunication(serialPort)
self.cmd = comm_packet_pb2.CommandPacket()
control_signal_cmd = self.cmd.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 0.0
def __del__(self):
self.wait()
def connectSlot(self, uiThread):
self.connect(uiThread, SIGNAL("send_cmds(PyQt_PyObject)"), self.processCmds)
def run(self):
while True:
# Repeatedly send our command packet and emit a signal containing the response
response = self.serialComm.commandArduino(self.cmd)
if response:
self.emit(SIGNAL('new_data_received(PyQt_PyObject)'), response)
def processCmds(self, cmd_packet):
self.cmd = cmd_packet
def __init__(self, port, y_length=0, x_length=0, only_send_data=False):
#self.frame_arr = np.zeros((y_length, x_length), dtype=np.uint8)
self.frame_arr = np.zeros((y_length, x_length))
self.telemetry = {}
self.stopped = False
self.network_thread = None
self.frame_ready = False
self.last_frame = self.frame_arr
self.only_send_data = only_send_data
self.frame_ready_callback = nothing
if(only_send_data):
self.serial_thread = SerialCommunication(self.frame_callback, port, get_raw_data_only=True)
self.network_thread = WebSocketConnection()
else:
if port == 'test':
print 'testing'
elif port.find('network:') != -1: # if contains 'network:' means that data comes from network
camera_name = port.split('network:')[1]
self.serial_thread = SerialThroughWebSocket(self.frame_callback, camera_name)
else:
self.serial_thread = SerialCommunication(self.frame_callback, port)
class Camera:
def __init__(self, port, y_length=0, x_length=0, only_send_data=False):
#self.frame_arr = np.zeros((y_length, x_length), dtype=np.uint8)
self.frame_arr = np.zeros((y_length, x_length))
self.telemetry = {}
self.stopped = False
self.network_thread = None
self.frame_ready = False
self.last_frame = self.frame_arr
self.only_send_data = only_send_data
self.frame_ready_callback = nothing
if(only_send_data):
self.serial_thread = SerialCommunication(self.frame_callback, port, get_raw_data_only=True)
self.network_thread = WebSocketConnection()
else:
if port == 'test':
print 'testing'
elif port.find('network:') != -1: # if contains 'network:' means that data comes from network
camera_name = port.split('network:')[1]
self.serial_thread = SerialThroughWebSocket(self.frame_callback, camera_name)
else:
self.serial_thread = SerialCommunication(self.frame_callback, port)
def frame_callback(self, data):
raise RuntimeError('frame_callback not implemented')
def stop(self):
self.stopped = True
try:
if self.network_thread is not None:
self.network_thread.stop()
if self.serial_thread is not None:
self.serial_thread.stop()
except AttributeError as e:
logging.error(e)
def on_frame_ready(self, callback):
""" When a frame is generated the given callback will be executed"""
self.frame_ready_callback = callback
class RobotTerminal(Cmd):
def __init__(self):
Cmd.__init__(self)
self.portName = "/dev/ttyUSB0"
self.serialComm = SerialCommunication(self.portName)
def do_exit(self, args):
"""Exits the terminal"""
raise SystemExit
def do_get_active_waypoint(self, args):
""" Gets the name of the active way point"""
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = WP_GET_ACTIVE
response = self.serialComm.commandArduino(cmd_packet)
if response:
print "The active waypoint is : " + response.ActiveWayPoint
else:
print " Command error"
return
def do_send_waypoint(self, args):
"""Prompts the user to enter a way-point and then sends it to the Arduino"""
self.send_waypoint()
return
def do_start_control(self, args):
"""Sends a sine wave control signal to the robot."""
try:
print " This command will send a sine wave to the robot. Paramterize the sine wave "
print "Enter amplitude : "
amplitude = float(raw_input())
print "Enter frequency : "
frequency = float(raw_input())
except ValueError:
print "Invalid amplitude or frequency. Both must be floats."
sine_wave = self.generateSineWave(amplitude, frequency)
for sample in sine_wave:
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = sample
response = self.serialComm.commandArduino(cmd_packet)
if response:
try:
print " Response signal : " + str(
response.RoverStatus[0].Value)
except IndexError:
print " "
def send_waypoint(self):
try:
print "WayPoint Name : "
way_point_name = raw_input()
print "Heading : "
way_point_heading = float(raw_input())
print "Distance : "
way_point_distance = float(raw_input())
except ValueError:
print "Invalid waypoint. Name must be a string less than 15 characters. Heading and distance must be a float."
way_point_cmd = comm_packet_pb2.CommandPacket()
way_point_cmd.WayPointCmd.Name = way_point_name
way_point_cmd.WayPointCmd.Heading = way_point_heading
way_point_cmd.WayPointCmd.Distance = way_point_distance
response = self.serialComm.commandArduino(way_point_cmd)
if self.isValidWayPoint(response):
print "WayPoint " + way_point_name + " successfully sent and processed."
else:
print "WayPoint " + way_point_name + " command was rejected."
def isValidWayPoint(self, response):
if response:
if response.RoverStatus[0].Id == WP_CMD_ACCEPT:
return True
return False
def generateSineWave(self,
amplitude=1.0,
frequency=1.0,
duration=1.0,
stepSize=0.02):
t = np.arange(0, duration, stepSize)
sine_wave = amplitude * np.sin((2 * np.pi * frequency) * t)
return sine_wave
def __init__(self):
Cmd.__init__(self)
self.portName = "/dev/ttyUSB0"
self.serialComm = SerialCommunication(self.portName)
def setUp(self):
self.testArticle = SerialCommunication("/dev/ttyUSB0")
class UtSerialCommunication(unittest.TestCase):
def setUp(self):
self.testArticle = SerialCommunication("/dev/ttyUSB0")
def test_sendCmdBadType(self):
logging.info("Sending an invalid type way point command")
cmd_packet = None
self.assertRaises(TypeError, self.testArticle.commandArduino,
cmd_packet)
cmd_packet = 3
self.assertRaises(TypeError, self.testArticle.commandArduino,
cmd_packet)
def test_sendNoNameWayPointCmd(self):
logging.info("Sending no name way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = 45
cmd_packet.WayPointCmd.Distance = 0.5
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendNoHeadingWayPointCmd(self):
logging.info("Sending no heading way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Distance = 0.5
cmd_packet.WayPointCmd.Name = "WayPoint A"
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendNoDistanceWayPointCmd(self):
logging.info("Sending no distance way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = 45.0
cmd_packet.WayPointCmd.Name = "WayPoint A"
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendEmptyWayPointCmd(self):
logging.info("Sending empty command")
cmd_packet = comm_packet_pb2.CommandPacket()
# OK to send an empty command as long as it's of type CommandPacket
# Just don't expect a response
response = self.testArticle.commandArduino(cmd_packet)
print response
def test_commandOneWayPoint(self):
response = self.helper_SendOneWayPoint(test_route[0])
self.helper_checkResponse(response)
def test_commandRoute(self):
for test_way_point in test_route:
response = self.helper_SendOneCmdPacket(test_way_point)
self.helper_checkResponse(response)
def test_commandControlSignal(self):
logging.info("Sending control signal command")
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 2.3456
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
def test_repeatedControlCommands20Hz(self):
logging.info("Sending repeated control signal commands 20 Hz")
# Set frequency to 20 Hz
self.testArticle.CommFrequency = 0.05
# No need to set comm frequency, default is 20 Hz.
self.helper_SendSineWaveControlSignal(stepSize=0.01)
# Expect more than 95% reliablility ... 100 packets sent, less than 5 failed
self.assertTrue(self.testArticle.NumFailedPackets < 5)
# ADDITIONAL RELIABILITY TESTS AT DIFFERENT FREQUENCIES. LEAVE IN FOR
# FUTURE RELIABILTY TESTING.
# def test_repeatedControlCommands25Hz(self):
# logging.info("Sending repeated control signal commands 25 Hz")
# # No need to set comm frequency, default is 20 Hz.
# self.helper_SendSineWaveControlSignal(stepSize=0.001)
# # Expect more than 95% reliablility ... 100 packets sent, less than 5 failed
# self.assertTrue(self.testArticle.NumFailedPackets < 5)
# def test_repeatedControlCommands50Hz(self):
# logging.info("Sending repeated control signal commands 50 Hz")
# # Set frequency to 50 Hz
# self.testArticle.CommFrequency = 0.02
# self.helper_SendSineWaveControlSignal(stepSize=0.001)
# # Expect 90% reliablility ... 1000 packets sent, no more than 100 failed
# self.assertTrue(self.testArticle.NumFailedPackets <= 100)
# def test_repeatedControlCommands100Hz(self):
# logging.info("Sending repeated control signal commands 100 Hz")
# # Set frequency to 100 Hz
# self.testArticle.CommFrequency = 0.01
# self.helper_SendSineWaveControlSignal(stepSize=0.001)
# # Expect 80% reliablility ... 1000 packets sent, no more than 200 failed
# self.assertTrue(self.testArticle.NumFailedPackets <= 200)
def helper_SendOneWayPoint(self, cmd_packet):
logging.info("Sending way point command : " +
cmd_packet.WayPointCmd.Name)
return self.helper_SendOneCmdPacket(cmd_packet)
def helper_SendOneCmdPacket(self, cmd_packet):
return self.testArticle.commandArduino(cmd_packet)
def helper_checkResponse(self, response):
if response:
logging.info("Success Packet # : " +
str(self.testArticle.NumReceivedPackets))
logging.info("Dumping received packet : \n" + str(response))
self.assertIsInstance(response, comm_packet_pb2.TelemetryPacket)
else:
logging.info("Failed Packet # : " +
str(self.testArticle.NumFailedPackets))
self.assertIsNone(response)
self.assertTrue(self.testArticle.NumFailedPackets >= 1)
def helper_SendSineWaveControlSignal(self,
amplitude=1.0,
frequency=1.0,
duration=1.0,
stepSize=0.01):
sine_wave = self.helper_generateSineWave(amplitude, frequency,
duration, stepSize)
for sample in sine_wave:
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = sample
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
def helper_generateSineWave(self,
amplitude=1.0,
frequency=1.0,
duration=1.0,
stepSize=0.1):
t = np.arange(0, duration, stepSize)
sine_wave = amplitude * np.sin((2 * np.pi * frequency) * t)
return sine_wave
class App(QMainWindow):
resized = pyqtSignal()
def __init__(self, width=1080, height=720):
super().__init__()
self.connectionStatus = False
self.width = width
self.height = height
self.initUI()
# self.initSignalsSlots()
self.qsObj = None
self.deviceName = ""
self.serialPort = None
self.currentTabMode = -1
self.sched = None
def initUI(self):
self.title = 'AutonomousFlight Configurator v1.0'
self.left = 0
self.top = 0
# self.width = 1080
# self.height = 720
self.toolbarheight = 65
self.setWindowTitle(self.title)
self.setGeometry(self.left, self.top, self.width, self.height)
self.centerWindow()
self.bundle_dir = os.path.dirname(os.path.abspath(__file__))
self.initWidgets()
self.setFocus()
def centerWindow(self):
frameGeo = self.frameGeometry()
screen = QApplication.desktop().screenNumber(QApplication.desktop().cursor().pos())
centerPoint = QApplication.desktop().screenGeometry(screen).center()
frameGeo.moveCenter(centerPoint)
self.move(frameGeo.topLeft())
def initWidgets(self):
# HeaderBar
self.initHeaderWidget = InitHeaderWidget(self)
self.initHeaderWidget.setGeometry(0, 0, self.width, 100)
self.initHeaderWidget.show()
self.initHeaderWidget.connectionStatusChangedSignal.connect(self.processInitConnection)
# Init CoverPage
self.coverPageWidget = CoverPageWidget(self)
self.coverPageWidget.setGeometry(0, 100, self.width, self.height-100)
self.coverPageWidget.show()
# Show
self.show()
def initWorkingWidget(self):
# Init StatusHeaderBar
self.statusHeaderWidget = StatusHeaderWidget(self, self.qsObj)
self.statusHeaderWidget.setGeometry(0, 0, self.width, 100)
self.statusHeaderWidget.connectionStatusChangedSignal.connect(self.processStatusConnection)
# self.statusHeaderWidget.hide()
# Init InfoTabs
self.infoTabsWidget = QTabWidget(self)
self.infoTabsWidget.setGeometry(0, 100, self.width, self.height-130)
# Vertical: 2
self.infoTabsWidget.setTabPosition(2)
self.tabNameList = ["CLI", "Overview", "Calibration", "Configure",
"ParameterSetting", "Mode", "Sensor", "Motor",
"Topology", "Blackbox", "SerialTerminal"]
self.tabObjectDict = {"CLI":CLIWidget(self),
"Overview":OverviewInfoWidget(self, self.qsObj),
"Calibration":CalibrationWidget(self, self.qsObj),
"Configure":ConfigureWidget(self, self.qsObj),
"ParameterSetting":ParameterSettingWidget(self, self.qsObj),
"Mode":FlightModeWidget(self, self.qsObj),
"Sensor":SensorWidget(self, self.qsObj),
"Motor":MotorInfoWidget(self, self.qsObj),
"Topology":TopologyWidget(self, self.qsObj),
"Blackbox":BlackboxWidget(self),
"SerialTerminal":SerialTerminalWidget(self)}
for key,value in self.tabObjectDict.items():
self.infoTabsWidget.addTab(value, key)
# self.infoTabsWidget.hide()
self.infoTabsWidget.currentChanged.connect(self.setTabMode)
# Init StatusBar
self.statusBar = QStatusBar()
# "Packet error:","I2C error:","Cycle Time:",
# "CPU Load:","MSP Version:", "MSP Load:",
# "MSP Roundtrip:","HW roundtrip:","Drop ratio:"
self.statusBarTopicList = ["Cycle Time:", "I2C error:", "CPU Load:", "MSP Version:"]
self.packetInfoLabel = QLabel(" | ".join(self.statusBarTopicList))
self.statusBar.addWidget(self.packetInfoLabel)
self.setStatusBar(self.statusBar)
# self.statusBar.hide()
def initSignalsSlots(self):
pass
def processInitConnection(self):
result = False
try:
# Start serial port
self.startSerialPort()
self.qsObj = QuadStates()
self.mspHandle = MSPv1(self.serialPort, self.qsObj)
result = self.mspHandle.preCheck()
# Respond to reboot command from MSPv1
self.mspHandle.rebootSignal.connect(self.reboot)
except:
print("Fail to open serial port.")
if result:
self.connectionStatus = True
self.debug()
self.initWorkingWidget()
# Send command to MSP class
self.statusHeaderWidget.statusDataRequestSignal.connect(self.mspHandle.processHeaderDataRequest)
self.tabObjectDict["CLI"].cliSignal.connect(self.mspHandle.cliCommandSwitch)
self.tabObjectDict["Overview"].overviewInfoSignal.connect(self.mspHandle.overviewInfoResponse)
self.tabObjectDict["Mode"].flightmodeSaveSignal.connect(self.mspHandle.processModeRangesSave)
self.tabObjectDict["Configure"].configureSaveSignal.connect(self.mspHandle.processConfigureSave)
self.tabObjectDict["ParameterSetting"].parameterSaveSignal.connect(self.mspHandle.processParameterSettingSave)
self.tabObjectDict["Calibration"].accCalibrateSignal.connect(self.mspHandle.processAccCalibrationRequest)
self.tabObjectDict["Calibration"].magCalibrateSignal.connect(self.mspHandle.processMagCalibrationRequest)
self.tabObjectDict["Calibration"].calibrationSaveSignal.connect(self.mspHandle.processCalibrationSave)
self.tabObjectDict["Sensor"].sensorDataRequestSignal.connect(self.mspHandle.processSensorDataRequest)
self.tabObjectDict["Motor"].motorDataRequestSignal.connect(self.mspHandle.processMotorDataRequest)
self.tabObjectDict["Mode"].modeRCDataRequestSignal.connect(self.mspHandle.processModeRCDataRequest)
self.tabObjectDict["Topology"].topologySaveSignal.connect(self.mspHandle.processTopologySave)
self.tabObjectDict["SerialTerminal"].serTerSignal.connect(self.mspHandle.processSerialTerminalRequest)
# MSP data changed will raise this signal to update labels in UI page
self.mspHandle.headerDataUpdateSignal.connect(self.headerPageUpdate)
self.mspHandle.cliFeedbackSignal.connect(self.tabObjectDict["CLI"].processFeedback)
self.mspHandle.calibrationFeedbackSignal.connect(self.tabObjectDict["Calibration"].processFeedback)
self.mspHandle.overviewDataUpdateSignal.connect(self.overviewPageUpdate)
self.mspHandle.sensorDataUpdateSignal.connect(self.sensorPageUpdate)
self.mspHandle.motorDataUpdateSignal.connect(self.motorPageUpdate)
self.mspHandle.modeRCDataUpdateSignal.connect(self.flightmodePageRCUpdate)
self.mspHandle.serTerFeedbackSignal.connect(self.tabObjectDict["SerialTerminal"].processFeedback)
# Change UI
self.initHeaderWidget.hide()
self.coverPageWidget.hide()
self.statusHeaderWidget.show()
self.infoTabsWidget.show()
self.infoTabsWidget.setCurrentIndex(0)
self.setTabMode(0)
self.statusBar.show()
# Start StatusHeader Timer
self.statusHeaderWidget.start()
def processStatusConnection(self):
self.connectionStatus = False
#
# self.checkSchedulerShutdown()
self.setTabMode(0)
self.statusHeaderWidget.stop()
# End serial port
self.closeSerialPort()
self.qsObj = None
# Change UI
self.statusHeaderWidget.hide()
self.infoTabsWidget.hide()
self.statusBar.hide()
self.initHeaderWidget.show()
self.coverPageWidget.show()
def keyPressEvent(self, event):
if self.currentTabMode == 0:
self.tabObjectDict["CLI"].keyPressed(event)
def debug(self):
print("---Debug---")
# print(self.qsObj.msp_activeboxes)
# print(self.qsObj.msp_boxids)
# print(len(self.qsObj.msp_boxids))
# print(self.qsObj.msp_boxnames)
# print(len(self.qsObj.msp_boxnames))
# print(self.qsObj.msp_mode_ranges)
# print(len(self.qsObj.msp_mode_ranges))
print("---Debug---")
pass
# Process reboot
def reboot(self):
# Step 1: Close current connection
self.processStatusConnection()
# Step 2: Wait for 0.3 seconds
self.waitTimer = QTimer()
self.waitTimer.setInterval(300)
self.waitTimer.setSingleShot(True)
# Step 3: Reconnect
self.waitTimer.timeout.connect(self.processInitConnection)
self.waitTimer.start(300)
# Page index
# Change corresponding index if the tabObjectDict is changed.
def setTabMode(self, index):
if self.currentTabMode != index:
if index == self.tabNameList.index("CLI"):
# Stop header widget update
self.statusHeaderWidget.stop()
elif self.currentTabMode == self.tabNameList.index("CLI"):
# Resume header widget update
self.statusHeaderWidget.start()
if index == self.tabNameList.index("Overview"):
# Overview Page
self.tabObjectDict["Overview"].start()
elif self.currentTabMode == self.tabNameList.index("Overview"):
self.tabObjectDict["Overview"].stop()
if index == self.tabNameList.index("Calibration"):
# Calibration
self.calibrationPageUpdate()
# Mode
if index == self.tabNameList.index("Mode"):
self.flightmodePageUpdate()
self.tabObjectDict["Mode"].start()
elif self.currentTabMode == self.tabNameList.index("Mode"):
self.tabObjectDict["Mode"].stop()
# Configure
if index == self.tabNameList.index("Configure"):
# Configure
self.configurePageUpdate()
if index == self.tabNameList.index("ParameterSetting"):
# Parameter Setting
self.parameterSettingPageUpdate()
if index == self.tabNameList.index("Sensor"):
# Sensor
self.tabObjectDict["Sensor"].start()
elif self.currentTabMode == self.tabNameList.index("Sensor"):
self.tabObjectDict["Sensor"].stop()
if index == self.tabNameList.index("Motor"):
# Motor
self.tabObjectDict["Motor"].start()
elif self.currentTabMode == self.tabNameList.index("Motor"):
self.tabObjectDict["Motor"].stop()
# Topology
if index == self.tabNameList.index("Topology"):
# Topology
self.topologyPageUpdate()
if index == self.tabNameList.index("Blackbox"):
# Blackbox
pass
if index == self.tabNameList.index("SerialTerminal"):
# Serial Terminal
# Stop header widget update
self.statusHeaderWidget.stop()
self.tabObjectDict["SerialTerminal"].start()
elif self.currentTabMode == self.tabNameList.index("SerialTerminal"):
self.tabObjectDict["SerialTerminal"].stop()
# Resume header widget update
self.statusHeaderWidget.start()
self.currentTabMode = index
def startSerialPort(self):
self.deviceName = self.initHeaderWidget.connectionMethodComboBox.currentText()
self.serialPort = SerialCommunication(self.deviceName)
def closeSerialPort(self):
self.serialPort.stopDevice()
self.serialPort = None
def headerPageUpdate(self, ind):
if ind == 0:
self.statusHeaderWidget.updateMMSV()
elif ind == 1:
self.statusHeaderWidget.updateSensorStatus()
def overviewPageUpdate(self, ind):
if ind == 0:
self.tabObjectDict["Overview"].updateAttitudeLabels()
elif ind == 1:
self.tabObjectDict["Overview"].updateArmingFlagLabels()
elif ind == 2:
self.tabObjectDict["Overview"].updateBatteryLabels()
elif ind == 3:
self.tabObjectDict["Overview"].updateCommunicationLabels()
elif ind == 4:
self.tabObjectDict["Overview"].updateGPSLabels()
# # Update header sensor lights.
# self.statusHeaderWidget.update(self.qsObj.msp_sensor_status)
# Update status bar
self.statusBarUpdate()
def statusBarUpdate(self):
self.statusBarTopicList = ["Cycle Time:", "I2C Error:", "CPU Load:", "MSP Version:"]
self.statusBarValueDict = {}
for field in self.statusBarTopicList:
data = None
if field == "Cycle Time:":
data = self.qsObj.msp_status_ex['cycleTime']
elif field == "I2C Error:":
data = self.qsObj.msp_status_ex['i2cError']
elif field == "CPU Load:":
data = self.qsObj.msp_status_ex['averageSystemLoadPercent']
elif field == "MSP Version:":
data = "1"
else:
pass
self.statusBarValueDict[field] = str(data)
labelString = "| "
for key,value in self.statusBarValueDict.items():
labelString = labelString + key + value + " | "
self.packetInfoLabel.setText(labelString)
def sensorPageUpdate(self, ind):
if ind == 0:
self.tabObjectDict["Sensor"].updateACCPlot()
self.tabObjectDict["Sensor"].updateGYROPlot()
self.tabObjectDict["Sensor"].updateMAGPlot()
elif ind == 1:
self.tabObjectDict["Sensor"].updateBAROPlot()
elif ind == 2:
self.tabObjectDict["Sensor"].updateRangeFinderPlot()
def motorPageUpdate(self, ind):
if ind == 0:
self.tabObjectDict["Motor"].updateMotorValues()
def flightmodePageRCUpdate(self, ind):
if ind == 0:
self.tabObjectDict["Mode"].updateCurrentValueLabels()
def flightmodePageUpdate(self):
self.tabObjectDict["Mode"].setRanges()
def parameterSettingPageUpdate(self):
self.mspHandle.processParameterSettingCheck()
self.tabObjectDict['ParameterSetting'].setValues()
def configurePageUpdate(self):
self.mspHandle.processConfigureCheck()
self.tabObjectDict['Configure'].setValues()
def calibrationPageUpdate(self):
self.mspHandle.processCalibrationCheck()
self.tabObjectDict['Calibration'].setValues()
def topologyPageUpdate(self):
self.mspHandle.processTopologyCheck()
self.tabObjectDict['Topology'].setValues()
def startSerialPort(self):
self.deviceName = self.initHeaderWidget.connectionMethodComboBox.currentText()
self.serialPort = SerialCommunication(self.deviceName)
'''
from xarm.wrapper import XArmAPI
from BodyGameRuntime import BodyGameRuntime
from ArmControl import ArmControl
from SerialCommunication import SerialCommunication
if __name__ == '__main__':
arm_left = XArmAPI('192.168.1.209')
arm_right = XArmAPI('192.168.1.151') # 连接控制器
arm_left.motion_enable(enable=True)
arm_right.motion_enable(enable=True) # 使能
arm_left.set_mode(0)
arm_right.set_mode(0) # 运动模式：位置控制模式
arm_left.set_state(state=0)
arm_right.set_state(state=0) # 运动状态：进入运动状态
s = SerialCommunication()
game = BodyGameRuntime()
arm = ArmControl(arm_left, arm_right)
while True:
a = s.ser()
if a == '88':
arm.initial()
game.run(arm)
arm.initial()
def setUp(self):
self.testArticle = SerialCommunication("./ttyclient")
self.testArticle.CommFrequency = 0.5
sleep(1.0)
class UtEmulatedSerialComm(unittest.TestCase):
def setUp(self):
self.testArticle = SerialCommunication("./ttyclient")
self.testArticle.CommFrequency = 0.5
sleep(1.0)
def test_sendCmdBadType(self):
logging.info("Sending an invalid type way point command")
cmd_packet = None
self.assertRaises(TypeError, self.testArticle.commandArduino,
cmd_packet)
cmd_packet = 3
self.assertRaises(TypeError, self.testArticle.commandArduino,
cmd_packet)
def test_sendNoNameWayPointCmd(self):
logging.info("Sending no name way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = 45
cmd_packet.WayPointCmd.Distance = 0.5
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendNoHeadingWayPointCmd(self):
logging.info("Sending no heading way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Distance = 0.5
cmd_packet.WayPointCmd.Name = "WayPoint A"
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendNoDistanceWayPointCmd(self):
logging.info("Sending no distance way point command")
cmd_packet = comm_packet_pb2.CommandPacket()
cmd_packet.WayPointCmd.Heading = 45.0
cmd_packet.WayPointCmd.Name = "WayPoint A"
self.assertRaises(IOError, self.testArticle.commandArduino, cmd_packet)
def test_sendEmptyWayPointCmd(self):
logging.info("Sending empty command")
cmd_packet = comm_packet_pb2.CommandPacket()
# OK to send an empty command as long as it's of type CommandPacket
# Just don't expect a response
response = self.testArticle.commandArduino(cmd_packet)
print response
def test_commandOneWayPoint(self):
response = self.helper_SendOneWayPoint(test_route[0])
self.helper_checkResponse(response)
def test_commandRoute(self):
for test_way_point in test_route:
response = self.helper_SendOneCmdPacket(test_way_point)
self.helper_checkResponse(response)
sleep(7)
def test_getActiveWayPoint(self):
logging.info("Sending get active waypoint command")
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = WP_GET_ACTIVE
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
logging.info("The active waypoint is : " + response.ActiveWayPoint)
def test_commandControlSignal(self):
logging.info("Sending control signal command")
cmd_packet = comm_packet_pb2.CommandPacket()
control_signal_cmd = cmd_packet.RoverCmds.add()
control_signal_cmd.Id = CTRL_ACTIVE
control_signal_cmd.Value = 2.3456
response = self.helper_SendOneCmdPacket(cmd_packet)
self.helper_checkResponse(response)
def helper_SendOneWayPoint(self, cmd_packet):
logging.info("Sending way point command : " +
cmd_packet.WayPointCmd.Name)
return self.helper_SendOneCmdPacket(cmd_packet)
def helper_SendOneCmdPacket(self, cmd_packet):
return self.testArticle.commandArduino(cmd_packet)
def helper_checkResponse(self, response):
if response:
logging.info("Success Packet # : " +
str(self.testArticle.NumReceivedPackets))
logging.info("Dumping received packet : \n" + str(response))
self.assertIsInstance(response, comm_packet_pb2.TelemetryPacket)
else:
logging.info("Failed Packet # : " +
str(self.testArticle.NumFailedPackets))
self.assertIsNone(response)
self.assertTrue(self.testArticle.NumFailedPackets >= 1)
def do_connect(self, args):
"""Connects to the robot. Required before sending commands."""
self.testArticle = SerialCommunication(self.portName)
def setUp(self):
self.serialComm = SerialCommunication("./ttyclient")
self.testArticle = CommandAndTracking()
self.testArticle.connectToWheelBot(self.serialComm)