diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index c2e763df148fbd389601beb65139454f66792f53..f7308aef0dea93bfa689dba5b342aa7ddb60320c 100644
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -57,6 +57,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
ViconData.msg
+ ControlParameters.msg
)
## Generate services in the 'srv' folder
@@ -145,7 +146,7 @@ include_directories(
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
-add_executable(ViconDataNode src/ViconDataNode.cpp)
+add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(PPSClient src/PPSClient.cpp)
## Rename C++ executable without prefix
@@ -165,8 +166,8 @@ add_executable(PPSClient src/PPSClient.cpp)
find_library(VICON_LIBRARY ViconDataStreamSDK_CPP HINTS lib/vicon)
-target_link_libraries(ViconDataNode ${catkin_LIBRARIES})
-target_link_libraries(ViconDataNode ${VICON_LIBRARY})
+target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
+target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
new file mode 100755
index 0000000000000000000000000000000000000000..a3f818b671a2459593abe22cea63ccf4fe0296e5
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import roslib
+roslib.load_manifest('d_fall_pps')
+import rospy
+from d_fall_pps.msg import ControlParameters
+
+
+# General import
+import time, sys
+import struct
+import logging
+
+# Add library
+#sys.path.append("lib")
+
+# CrazyFlie client imports
+import cflib
+
+from cflib.crazyflie import Crazyflie
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+
+import cflib.drivers.crazyradio
+
+# Logging import
+from cflib.crazyflie.log import LogConfig
+
+# Logging settings
+logging.basicConfig(level=logging.ERROR)
+
+class SimpleClient:
+ """
+ Example script that runs several threads that read Vicon measurements
+ from a file and send it together with the setpoints to the Crazyflie.
+ It also employs a keyboard event detector that allows the user to
+ manipulate the setpoints with keys.
+ """
+ def __init__(self, link_uri):
+
+ # Setpoints to be sent to the CrazyFlie
+ self.roll = 0.0
+ self.pitch = 0.0
+ self.yaw = 0.0
+ self.motor1cmd = 0.0
+ self.motor2cmd = 0.0
+ self.motor3cmd = 0.0
+ self.motor4cmd = 0.0
+
+ # Initialize the CrazyFlie and add callbacks
+ self._cf = Crazyflie()
+
+ # Add callbacks that get executed depending on the connection status.
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+
+ # Connect to the Crazyflie
+ print "Connecting to %s" % link_uri
+ self._cf.open_link(link_uri)
+
+
+ def _connected(self, link_uri):
+ """
+ This callback is executed as soon as the connection to the
+ quadrotor is established.
+ """
+ #print "Connection to %s successful: " % link_uri
+ rospy.loginfo("Connection to %s successful: " % link_uri)
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the specified address)"""
+ #print "Connection to %s failed: %s" % (link_uri, msg)
+ rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
+
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ #print "Connection to %s lost: %s" % (link_uri, msg)
+ rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ #print "Disconnected from %s" % link_uri
+ rospy.logwarn("Disconnected from %s" % link_uri)
+
+
+ def _send_commands(self,cmd1,cmd2,cmd3,cmd4):
+
+ # Send setpoints at the given frequency.
+ # Fill the CRTP packet with the setpoints and send it to the stabilizer
+ pk = CRTPPacket()
+ pk.port = CRTPPort.STABILIZER
+ pk.data = struct.pack('<ffff', cmd1,cmd2,cmd3,cmd4)
+ self._cf.send_packet(pk)
+ #print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
+
+ def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
+ pk = CRTPPacket()
+ pk.port = CRTPPort.COMMANDER
+ pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+ self._cf.send_packet(pk)
+
+ # self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+ # print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
+
+def subscriberControllerOutputCallback(data):
+ #cf_client._send_commands(data.cmd1,data.cmd2,data.cmd3,data.cmd4)
+ cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust,data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
+ #rospy.loginfo(data.onboardControllerType);
+ #rospy.loginfo(data.motorCmd1);
+ #rospy.logdebug_throttle(2,"sending motor commands to crazyflie: ")
+
+
+
+
+if __name__ == '__main__':
+ rospy.init_node('CrazyRadio', anonymous=True)
+ cflib.crtp.init_drivers(enable_debug_driver=False)
+
+ while not rospy.is_shutdown():
+
+ # Initialize the low-level drivers (don't list the debug drivers)
+
+ # Scan for Crazyflies and use the first one found
+ rospy.loginfo("Scanning interfaces for Crazyflies...")
+ available=[]
+ available = cflib.crtp.scan_interfaces()
+ rospy.loginfo("Crazyflies found:")
+ for i in available:
+ print i[0]
+ if len(available) > 0:
+ # uri would can be specified directly, as for example: radio://0/70/250K
+ # instead of available[0][0]
+ global cf_client
+ cf_client = SimpleClient('radio://0/80/250K')
+ #cf_client = SimpleClient(available[0][0])
+ time.sleep(5.0)
+ rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
+
+ rospy.spin()
+ else:
+ rospy.logerr("No Crazyflies found, cannot run example")
+
+ #inp=raw_input('press any key');
+
+
+ time.sleep(0.5)
+
+ cf_client._cf.close_link()
+
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..e3208939bd4ed85fd863be309fa7d7a5fa4eab97
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.py
@@ -0,0 +1,44 @@
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+from .cfclient import main
+
+try:
+ from .version import VERSION
+except:
+ try:
+ import subprocess
+ VERSION = subprocess.check_output(["git", "describe"])
+ except:
+ VERSION = "dev"
+
+ try:
+ import subprocess
+ ret = subprocess.call(["git", "diff", "--quiet", "HEAD"])
+ if ret > 0:
+ VERSION += "+"
+ except:
+ VERSION += "+"
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..8c79f1b6984af0d16b764ba41f46662cedd7fbf9
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.py
new file mode 100755
index 0000000000000000000000000000000000000000..81b3594b3da6b5bd71548f0d874db8b0e76cbb5b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.py
@@ -0,0 +1,137 @@
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""Initialization of the PC Client GUI."""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['']
+
+import sys
+import os
+import argparse
+import datetime
+
+import logging
+
+
+def main():
+
+ """
+ Check starting conditions and start GUI.
+
+ First, check command line arguments and start loggers. Set log levels. Try
+ all imports and exit verbosely if a library is not found. Disable outputs
+ to stdout and start the GUI.
+ """
+
+ # Set ERROR level for PyQt4 logger
+ qtlogger = logging.getLogger('PyQt4')
+ qtlogger.setLevel(logging.ERROR)
+
+ parser = argparse.ArgumentParser(description="cfclient - "
+ "Crazyflie graphical control client")
+ parser.add_argument('--debug', '-d', nargs=1, default='info', type=str,
+ help="set debug level "
+ "[minimal, info, debug, debugfile]")
+ args = parser.parse_args()
+ globals().update(vars(args))
+
+ cflogger = logging.getLogger('')
+
+ # Set correct logging fuctionality according to commandline
+ if ("debugfile" in debug):
+ logging.basicConfig(level=logging.DEBUG)
+ # Add extra format options for file logger (thread and time)
+ formatter = logging.Formatter('%(asctime)s:%(threadName)s:%(name)'
+ 's:%(levelname)s:%(message)s')
+ filename = "debug-%s.log" % datetime.datetime.now()
+ filehandler = logging.FileHandler(filename)
+ filehandler.setLevel(logging.DEBUG)
+ filehandler.setFormatter(formatter)
+ cflogger.addHandler(filehandler)
+ elif ("debug" in debug):
+ logging.basicConfig(level=logging.DEBUG)
+ elif ("minimal" in debug):
+ logging.basicConfig(level=logging.WARNING)
+ elif ("info" in debug):
+ logging.basicConfig(level=logging.INFO)
+
+ logger = logging.getLogger(__name__)
+
+ logger.debug("Using config path {}".format(sys.path[1]))
+ logger.debug("sys.path={}".format(sys.path))
+
+ # Try all the imports used in the project here to control what happens....
+ try:
+ import usb
+ except ImportError:
+ logger.critical("No pyusb installation found, exiting!")
+ sys.exit(1)
+
+ if not sys.platform.startswith('linux'):
+ try:
+ import sdl2
+ except ImportError:
+ logger.critical("No pysdl2 installation found, exiting!")
+ sys.exit(1)
+
+ try:
+ import PyQt4
+ except ImportError:
+ logger.critical("No PyQT4 installation found, exiting!")
+ sys.exit(1)
+
+ # Disable printouts from STL
+ if os.name == 'posix':
+ stdout = os.dup(1)
+ os.dup2(os.open('/dev/null', os.O_WRONLY), 1)
+ sys.stdout = os.fdopen(stdout, 'w')
+ logger.info("Disabling STL printouts")
+
+ if os.name == 'nt':
+ stdout = os.dup(1)
+ os.dup2(os.open('NUL', os.O_WRONLY), 1)
+ sys.stdout = os.fdopen(stdout, 'w')
+ logger.info("Disabling STL printouts")
+
+ # Start up the main user-interface
+ from ui.main import MainUI
+ from PyQt4.QtGui import QApplication, QIcon
+ app = QApplication(sys.argv)
+
+ app.setWindowIcon(QIcon(sys.path[0] + "/cfclient/icon-256.png"))
+ # Make sure the right icon is set in Windows 7+ taskbar
+ if os.name == 'nt':
+ import ctypes
+ try:
+ myappid = 'mycompany.myproduct.subproduct.version'
+ ctypes.windll.shell32.SetCurrentProcessExplicitAppUserModelID(myappid)
+ except Exception:
+ pass
+
+ main_window = MainUI()
+ main_window.show()
+ sys.exit(app.exec_())
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..6054b91a78b93015ac313504f2b88b0f3a1b2edb
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/cfclient.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/config.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/config.json
new file mode 100755
index 0000000000000000000000000000000000000000..c78d5b0e7948a211fd64f5867b3268ad50c42bf8
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/config.json
@@ -0,0 +1,34 @@
+{
+ "writable" : {
+ "input_device": "",
+ "link_uri": "",
+ "flightmode": "Normal",
+ "open_tabs": "Flight Control",
+ "trim_pitch": 0.0,
+ "slew_limit": 45,
+ "slew_rate": 30,
+ "trim_roll": 0.0,
+ "max_thrust": 80,
+ "min_thrust": 25,
+ "max_yaw": 200,
+ "max_rp": 30,
+ "client_side_xmode": false,
+ "auto_reconnect": false,
+ "device_config_mapping": {},
+ "enable_debug_driver": false,
+ "input_device_blacklist": "(VirtualBox|VMware)",
+ "ui_update_period": 100,
+ "enable_zmq_input": false
+ },
+ "read-only" : {
+ "normal_slew_limit": 45,
+ "normal_slew_rate": 30,
+ "normal_max_thrust": 80,
+ "normal_min_thrust": 25,
+ "normal_max_yaw": 200,
+ "normal_max_rp": 30,
+ "default_cf_channel": 10,
+ "default_cf_speed": 0,
+ "default_cf_trim": 0
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Generic_OS_X.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Generic_OS_X.json
new file mode 100755
index 0000000000000000000000000000000000000000..da3c935fc7c317e912ae54a45f7c10c415369189
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Generic_OS_X.json
@@ -0,0 +1,18 @@
+{
+ "inputconfig": {
+ "inputdevice":
+ {"name": "Generic 2-axis gamepad on OS X", "updateperiod":10,
+ "axis": [
+ {"name":"Roll", "type":"Input.AXIS", "id":3, "scale":1.0, "key":"roll"},
+ {"name":"Pitch", "type":"Input.AXIS", "id":4, "scale":-1.0, "key":"pitch"},
+ {"name":"Yaw", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"yaw"},
+ {"name":"Thrust", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"thrust"},
+ {"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
+ {"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
+ {"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
+ {"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
+ {"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
+ {"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
+ ]}
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Joystick.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Joystick.json
new file mode 100755
index 0000000000000000000000000000000000000000..00e1a5edcc90034168d368d62ee81c1a51bfe799
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/Joystick.json
@@ -0,0 +1,95 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "springythrottle": false,
+ "name": "Joystick",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 3,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 2,
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 0,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.HAT",
+ "id": 0,
+ "key": "trim",
+ "name": "trim"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 4,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 5,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 1,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 2,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 3,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 0,
+ "key": "althold",
+ "name": "althold"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 6,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_1.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_1.json
new file mode 100755
index 0000000000000000000000000000000000000000..ae5cc3dfa6f3dc27e2d2882caf6857093245a15b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_1.json
@@ -0,0 +1,19 @@
+{
+ "inputconfig": {
+ "inputdevice":
+ {"name": "Playstation 3 Mode 1", "updateperiod":10,
+ "axis": [
+ {"name":"Roll", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"roll"},
+ {"name":"Pitch", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"pitch"},
+ {"name":"Yaw", "type":"Input.AXIS", "id":2, "scale":1.0, "key":"yaw"},
+ {"name":"Thrust", "type":"Input.AXIS", "id":3, "scale":-1.0, "key":"thrust"},
+ {"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
+ {"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
+ {"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
+ {"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
+ {"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
+ {"name":"Altitude hold", "type":"Input.BUTTON", "id":10, "scale":1.0, "key":"althold"},
+ {"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
+ ]}
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_2.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_2.json
new file mode 100755
index 0000000000000000000000000000000000000000..4c7225deffbb5cf65dbfa60f6ef22efcdf1a936f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_2.json
@@ -0,0 +1,18 @@
+{
+ "inputconfig": {
+ "inputdevice":
+ {"name": "Playstation 3 Mode 2", "updateperiod":10,
+ "axis": [
+ {"name":"Roll", "type":"Input.AXIS", "id":2, "scale":1.0, "key":"roll"},
+ {"name":"Pitch", "type":"Input.AXIS", "id":3, "scale":-1.0, "key":"pitch"},
+ {"name":"Yaw", "type":"Input.AXIS", "id":0, "scale":1.0, "key":"yaw"},
+ {"name":"Thrust", "type":"Input.AXIS", "id":1, "scale":-1.0, "key":"thrust"},
+ {"name":"Pitch Cal+", "type":"Input.BUTTON", "id":6, "scale":-1.0, "key":"pitchNeg"},
+ {"name":"Pitch Cal-", "type":"Input.BUTTON", "id":4, "scale":1.0, "key":"pitchPos"},
+ {"name":"Roll Cal +", "type":"Input.BUTTON", "id":5, "scale":1.0, "key":"rollPos"},
+ {"name":"Roll Cal -", "type":"Input.BUTTON", "id":7, "scale":-1.0, "key":"rollNeg"},
+ {"name":"Killswtich", "type":"Input.BUTTON", "id":14, "scale":1.0, "key":"estop"},
+ {"name":"Exit", "type":"Input.BUTTON", "id":12, "scale":1.0, "key":"exit"}
+ ]}
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_3.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_3.json
new file mode 100755
index 0000000000000000000000000000000000000000..18fea8441bbb5dd03427ce8fde66516c725eac68
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS3_Mode_3.json
@@ -0,0 +1,83 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "name": "PS3_Mode_3",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "ids": [
+ 12,
+ 13
+ ],
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 2,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 3,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 6,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 4,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 14,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 7,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 5,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 12,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_1.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_1.json
new file mode 100755
index 0000000000000000000000000000000000000000..15e0010d2107c7e468bcb406b57ab8dd03d624fe
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_1.json
@@ -0,0 +1,87 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "name": "PS4_Mode_1",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 5,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 2,
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 0,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 1,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 3,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 12,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 0,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 2,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 5,
+ "key": "althold",
+ "name": "althold"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 8,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_2.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_2.json
new file mode 100755
index 0000000000000000000000000000000000000000..f369ebdc43d38d99b62872dab2273800f4cd9cac
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_Mode_2.json
@@ -0,0 +1,87 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "name": "PS4_Mode_2",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 0,
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 2,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 5,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 1,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 3,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 7,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 0,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 2,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 4,
+ "key": "althold",
+ "name": "althold"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 8,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_shoulder_btns_yaw.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_shoulder_btns_yaw.json
new file mode 100755
index 0000000000000000000000000000000000000000..6a14488d3815e15d24a0e1a8bc9a3b86fc173fc1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/PS4_shoulder_btns_yaw.json
@@ -0,0 +1,91 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "name": "PS4_shoulder_btns_yaw",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 5,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "offset": 1.0,
+ "scale": 0.5,
+ "type": "Input.AXIS",
+ "ids": [
+ 3,
+ 4
+ ],
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 0,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 22,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 21,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": 23,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 24,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 5,
+ "key": "althold",
+ "name": "althold"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/xbox360_mode1.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/xbox360_mode1.json
new file mode 100755
index 0000000000000000000000000000000000000000..0ca5d032a0763a09979b57d9803a219fefac02a0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/input/xbox360_mode1.json
@@ -0,0 +1,87 @@
+{
+ "inputconfig": {
+ "inputdevice": {
+ "updateperiod": 10,
+ "name": "xbox360_mode1_linux",
+ "axis": [
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 4,
+ "key": "thrust",
+ "name": "thrust"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 3,
+ "key": "yaw",
+ "name": "yaw"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.AXIS",
+ "id": 0,
+ "key": "roll",
+ "name": "roll"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.AXIS",
+ "id": 1,
+ "key": "pitch",
+ "name": "pitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "pitchNeg",
+ "name": "pitchNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "pitchPos",
+ "name": "pitchPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "estop",
+ "name": "killswitch"
+ },
+ {
+ "scale": -1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "rollNeg",
+ "name": "rollNeg"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "rollPos",
+ "name": "rollPos"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": 5,
+ "key": "althold",
+ "name": "althold"
+ },
+ {
+ "scale": 1.0,
+ "type": "Input.BUTTON",
+ "id": -1,
+ "key": "exit",
+ "name": "exitapp"
+ }
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/log/stabilizer.json b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/log/stabilizer.json
new file mode 100755
index 0000000000000000000000000000000000000000..c131b81782709f602aa32760d839c6132efc94aa
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/configs/log/stabilizer.json
@@ -0,0 +1,11 @@
+{
+ "logconfig": {
+ "logblock":
+ {"name": "Stabilizer", "period":20,
+ "variables": [
+ {"name":"stabilizer.roll", "type":"TOC", "stored_as":"float", "fetch_as":"float"},
+ {"name":"stabilizer.pitch", "type":"TOC", "stored_as":"float", "fetch_as":"float"},
+ {"name":"stabilizer.yaw", "type":"TOC", "stored_as":"float", "fetch_as":"float"}
+ ]}
+ }
+}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/icon-256.png b/pps_ws/src/d_fall_pps/crazyradio/cfclient/icon-256.png
new file mode 100755
index 0000000000000000000000000000000000000000..8c9fc927c683fa24956eb0bce233a808fe6d7f15
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/icon-256.png differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..51d3a306bf255163bf9df29bbe2a70261155ce1c
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Create a plugin helper that is passed to all the tabs and toolboxes for easy #
+access to objects that are needed.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = []
+
+from cfclient.ui.pluginhelper import PluginHelper
+
+pluginhelper = PluginHelper()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..c104e7ddfe707c932c60f7d3a900cc8a2627a688
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..5f70eceb5bdffb7217e4898413ba04e8b69a74cb
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.py
new file mode 100755
index 0000000000000000000000000000000000000000..b89b6176e0cad447d0eb233200a0c1c8fb1ec4b0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.py
@@ -0,0 +1,216 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The about dialog.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['AboutDialog']
+
+import sys
+
+from PyQt4 import Qt, QtCore, QtGui, uic
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+from PyQt4.Qt import *
+
+import cfclient
+
+import cflib.crtp
+
+(about_widget_class,
+about_widget_base_class) = (uic.loadUiType(sys.path[0] +
+ '/cfclient/ui/dialogs/about.ui'))
+
+DEBUG_INFO_FORMAT = """
+<b>Cfclient</b><br>
+Cfclient version: {version}<br>
+System: {system}<br>
+<br>
+<b>Interface status</b><br>
+{interface_status}
+<br>
+<b>Input readers</b><br>
+{input_readers}
+<br>
+<b>Input devices</b><br>
+{input_devices}
+<br>
+<b>Crazyflie</b><br>
+Connected: {uri}<br>
+Firmware: {firmware}<br>
+<b>Sensors found</b><br>
+{imu_sensors}
+<b>Sensors tests</b><br>
+{imu_sensor_tests}
+"""
+
+INTERFACE_FORMAT = "{}: {}<br>"
+INPUT_READER_FORMAT = "{} ({} devices connected)<br>"
+DEVICE_FORMAT = "{}: ({}) {}<br>"
+IMU_SENSORS_FORMAT = "{}: {}<br>"
+SENSOR_TESTS_FORMAT = "{}: {}<br>"
+FIRMWARE_FORMAT = "{:x}{:x} ({})"
+
+CREDITS_FORMAT = U"""
+<b>Contributions</b><br>
+{contribs}
+<br><br>
+<b>Used libraries</b><br>
+<a href="http://qt-project.org/">QT</a><br>
+<a href="http://www.riverbankcomputing.co.uk/software/pyqt/intro">PyQT</a><br>
+<a href="http://pysdl2.readthedocs.org">PySDL2</a><br>
+<a href="http://www.pyqtgraph.org/">PyQtGraph</a><br>
+<a href="http://marble.kde.org/">KDE Marble</a><br>
+<a href="http://sourceforge.net/projects/pyusb/">PyUSB</a><br>
+<a href="http://www.python.org/">Python</a><br>
+"""
+
+class AboutDialog(QtGui.QWidget, about_widget_class):
+
+ _disconnected_signal = pyqtSignal(str)
+
+ """Crazyflie client About box for debugging and information"""
+ def __init__(self, helper, *args):
+ super(AboutDialog, self).__init__(*args)
+ self.setupUi(self)
+ self._close_button.clicked.connect(self.close)
+ self._name_label.setText(
+ self._name_label.text().replace('#version#',
+ cfclient.VERSION))
+
+ self._interface_text = ""
+ self._imu_sensors_text = ""
+ self._imu_sensor_test_text = ""
+ self._uri = None
+ self._fw_rev0 = None
+ self._fw_rev1 = None
+ self._fw_modified = None
+ self._helper = helper
+
+ helper.cf.param.add_update_callback(group="imu_sensors",
+ cb=self._imu_sensors_update)
+ helper.cf.param.add_update_callback(group="imu_tests",
+ cb=self._imu_sensor_tests_update)
+ helper.cf.param.add_update_callback(group="firmware",
+ cb=self._firmware_update)
+ helper.cf.connected.add_callback(self._connected)
+
+ self._disconnected_signal.connect(self._disconnected)
+ helper.cf.disconnected.add_callback(self._disconnected_signal.emit)
+
+ # Open the Credits file and show it in the UI
+ credits = U""
+ try:
+ with open("CREDITS.txt", 'r') as f:
+ for line in f:
+ credits += U"{}<br>".format(line.decode("UTF-8"))
+ except IOError:
+ credits = U""
+
+ self._credits.setHtml(
+ CREDITS_FORMAT.format(contribs=credits)
+ )
+
+ def showEvent(self, event):
+ """Event when the about box is shown"""
+ self._interface_text = ""
+ interface_status = cflib.crtp.get_interfaces_status()
+ for key in interface_status.keys():
+ self._interface_text += INTERFACE_FORMAT.format(key,
+ interface_status[key])
+ firmware = None
+
+ self._device_text = ""
+ devs = self._helper.inputDeviceReader.available_devices()
+ for d in devs:
+ self._device_text += DEVICE_FORMAT.format(d.reader_name,
+ d.id,
+ d.name)
+ if len(self._device_text) == 0:
+ self._device_text = "None<br>"
+
+ self._input_readers_text = ""
+ #readers = self._helper.inputDeviceReader.getAvailableDevices()
+ for reader in cfclient.utils.input.inputreaders.initialized_readers:
+ self._input_readers_text += INPUT_READER_FORMAT.format(reader.name,
+ len(reader.devices()))
+ if len(self._input_readers_text) == 0:
+ self._input_readers_text = "None<br>"
+
+ if self._uri:
+ firmware = FIRMWARE_FORMAT.format(self._fw_rev0, self._fw_rev1,
+ "MODIFIED" if self._fw_modified else "CLEAN")
+ self._debug_out.setHtml(
+ DEBUG_INFO_FORMAT.format(version=cfclient.VERSION,
+ system=sys.platform,
+ interface_status=self._interface_text,
+ input_devices=self._device_text,
+ input_readers=self._input_readers_text,
+ uri = self._uri,
+ firmware = firmware,
+ imu_sensors=self._imu_sensors_text,
+ imu_sensor_tests=
+ self._imu_sensor_test_text))
+
+ def _connected(self, uri):
+ """Callback when Crazyflie is connected"""
+ self._uri = uri
+
+ def _firmware_update(self, name, value):
+ """Callback for firmware parameters"""
+ if "revision0" in name:
+ self._fw_rev0 = eval(value)
+ if "revision1" in name:
+ self._fw_rev1 = eval(value)
+ if "modified" in name:
+ self._fw_modified = eval(value)
+
+ def _imu_sensors_update(self, name, value):
+ """Callback for sensor found parameters"""
+ param = name[name.index('.') + 1:]
+ if not param in self._imu_sensors_text:
+ self._imu_sensors_text += IMU_SENSORS_FORMAT.format(param,
+ eval(value))
+
+ def _imu_sensor_tests_update(self, name, value):
+ """Callback for sensor test parameters"""
+ param = name[name.index('.') + 1:]
+ if not param in self._imu_sensor_test_text:
+ self._imu_sensor_test_text += SENSOR_TESTS_FORMAT.format(param,
+ eval(value))
+
+ def _disconnected(self, uri):
+ """Callback for Crazyflie disconnected"""
+ self._interface_text = ""
+ self._imu_sensors_text = ""
+ self._imu_sensor_test_text = ""
+ self._uri = None
+ self._fw_rev1 = None
+ self._fw_rev0 = None
+ self._fw_modified = None
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..ffef3f1194f284714b174e6d7a4759edd5e8822f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.ui
new file mode 100755
index 0000000000000000000000000000000000000000..9fd190e5bde23c3aefe88261d642c0a3abb06f23
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/about.ui
@@ -0,0 +1,139 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>511</width>
+ <height>338</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>About Crazyflie Client</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTabWidget" name="tabWidget">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <widget class="QWidget" name="About">
+ <attribute name="title">
+ <string>About</string>
+ </attribute>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Copyright (c) 2011-2013, Bitcraze AB</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string><html><head/><body><p><span style=" font-size:12pt;">The Crazyflie Nano Quadcopter client is a multi-platform client<br/>for controlling, bootloading and logging the Crazyflie Nano<br/>Quadcopter. For more info visit our homepage.</span></p><p><a href="http://www.bitcraze.se"><span style=" text-decoration: underline; color:#0000ff;">Bitcraze Homepage</span></a><span style=" font-size:12pt;"><br/></span><a href="http://wiki.bitcraze.se"><span style=" text-decoration: underline; color:#0000ff;">Bitcraze Wiki</span></a><span style=" font-size:12pt;"><br/></span><a href="http://forum.bitcraze.se"><span style=" text-decoration: underline; color:#0000ff;">Bitcraze Forum</span></a></p><p><br/></p></body></html></string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="_name_label">
+ <property name="text">
+ <string><html><head/><body><p><span style=" font-size:14pt; font-weight:600;">Cfclient #version#</span></p></body></html></string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="Credits">
+ <attribute name="title">
+ <string>Credits</string>
+ </attribute>
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <widget class="QTextEdit" name="_credits"/>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="Debug">
+ <attribute name="title">
+ <string>Debug</string>
+ </attribute>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QTextBrowser" name="_debug_out"/>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="0">
+ <spacer name="horizontalSpacer">
+ <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 row="1" column="1">
+ <widget class="QPushButton" name="_close_button">
+ <property name="text">
+ <string>Close</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.py
new file mode 100755
index 0000000000000000000000000000000000000000..b593e11da359a88411192472832a2976714652cf
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.py
@@ -0,0 +1,287 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The bootloader dialog is used to update the Crazyflie firmware and to
+read/write the configuration block in the Crazyflie flash.
+"""
+from cflib.bootloader import Bootloader
+
+__author__ = 'Bitcraze AB'
+__all__ = ['BootloaderDialog']
+
+import struct
+import sys
+import time
+
+import logging
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cfclient.ui.tab import Tab
+
+import cflib.crtp
+
+from cflib.bootloader.cloader import Cloader
+
+service_dialog_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/dialogs/bootloader.ui")[0]
+
+
+class UIState:
+ DISCONNECTED = 0
+ CONNECTING = 5
+ CONNECT_FAILED = 1
+ COLD_CONNECT = 2
+ FLASHING = 3
+ RESET = 4
+
+
+class BootloaderDialog(QtGui.QWidget, service_dialog_class):
+ """Tab for update the Crazyflie firmware and for reading/writing the config
+ block in flash"""
+ def __init__(self, helper, *args):
+ super(BootloaderDialog, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Service"
+ self.menuName = "Service"
+
+ # self.tabWidget = tabWidget
+ self.helper = helper
+
+ # self.cf = crazyflie
+ self.clt = CrazyloadThread()
+
+ # Connecting GUI signals (a pity to do that manually...)
+ self.imagePathBrowseButton.clicked.connect(self.pathBrowse)
+ self.programButton.clicked.connect(self.programAction)
+ self.verifyButton.clicked.connect(self.verifyAction)
+ self.coldBootButton.clicked.connect(self.initiateColdboot)
+ self.resetButton.clicked.connect(self.resetCopter)
+ self._cancel_bootloading.clicked.connect(self.close)
+
+ # connecting other signals
+ self.clt.programmed.connect(self.programDone)
+ self.clt.verified.connect(self.verifyDone)
+ self.clt.statusChanged.connect(self.statusUpdate)
+ # self.clt.updateBootloaderStatusSignal.connect(
+ # self.updateBootloaderStatus)
+ self.clt.connectingSignal.connect(lambda:
+ self.setUiState(UIState.CONNECTING))
+ self.clt.connectedSignal.connect(lambda:
+ self.setUiState(UIState.COLD_CONNECT))
+ self.clt.failed_signal.connect(lambda m: self._ui_connection_fail(m))
+ self.clt.disconnectedSignal.connect(lambda:
+ self.setUiState(UIState.DISCONNECTED))
+
+ self.clt.start()
+
+ def _ui_connection_fail(self, message):
+ self.setStatusLabel(message)
+ self.coldBootButton.setEnabled(True)
+
+ def setUiState(self, state):
+ if (state == UIState.DISCONNECTED):
+ self.resetButton.setEnabled(False)
+ self.programButton.setEnabled(False)
+ self.setStatusLabel("Not connected")
+ self.coldBootButton.setEnabled(True)
+ self.progressBar.setTextVisible(False)
+ self.progressBar.setValue(0)
+ self.statusLabel.setText('Status: <b>IDLE</b>')
+ self.imagePathLine.setText("")
+ elif (state == UIState.CONNECTING):
+ self.resetButton.setEnabled(False)
+ self.programButton.setEnabled(False)
+ self.setStatusLabel("Trying to connect cold bootloader, restart "
+ "the Crazyflie to connect")
+ self.coldBootButton.setEnabled(False)
+ elif (state == UIState.CONNECT_FAILED):
+ self.setStatusLabel("Connecting to bootloader failed")
+ self.coldBootButton.setEnabled(True)
+ elif (state == UIState.COLD_CONNECT):
+ self.resetButton.setEnabled(True)
+ self.programButton.setEnabled(True)
+ self.setStatusLabel("Connected to bootloader")
+ self.coldBootButton.setEnabled(False)
+ elif (state == UIState.RESET):
+ self.setStatusLabel("Resetting to firmware, disconnected")
+ self.resetButton.setEnabled(False)
+ self.programButton.setEnabled(False)
+ self.coldBootButton.setEnabled(False)
+ self.imagePathLine.setText("")
+
+ def setStatusLabel(self, text):
+ self.connectionStatus.setText("Status: <b>%s</b>" % text)
+
+ def connected(self):
+ self.setUiState(UIState.COLD_CONNECT)
+
+ def connectionFailed(self):
+ self.setUiState(UIState.CONNECT_FAILED)
+
+ def resetCopter(self):
+ self.clt.resetCopterSignal.emit()
+ self.setUiState(UIState.RESET)
+
+ def updateConfig(self, channel, speed, rollTrim, pitchTrim):
+ self.rollTrim.setValue(rollTrim)
+ self.pitchTrim.setValue(pitchTrim)
+ self.radioChannel.setValue(channel)
+ self.radioSpeed.setCurrentIndex(speed)
+
+ def closeEvent(self, event):
+ self.setUiState(UIState.RESET)
+ self.clt.resetCopterSignal.emit()
+
+ @pyqtSlot()
+ def pathBrowse(self):
+ filename = ""
+ # Fix for crash in X on Ubuntu 14.04
+ filename = QtGui.QFileDialog.getOpenFileName()
+ if filename != "":
+ self.imagePathLine.setText(filename)
+ pass
+
+ @pyqtSlot()
+ def programAction(self):
+ # self.setStatusLabel("Initiate programming")
+ self.resetButton.setEnabled(False)
+ self.programButton.setEnabled(False)
+ self.imagePathBrowseButton.setEnabled(False)
+ if self.imagePathLine.text() != "":
+ self.clt.program.emit(self.imagePathLine.text(),
+ self.verifyCheckBox.isChecked())
+ else:
+ msgBox = QtGui.QMessageBox()
+ msgBox.setText("Please choose an image file to program.")
+
+ msgBox.exec_()
+
+ @pyqtSlot()
+ def verifyAction(self):
+ self.statusLabel.setText('Status: <b>Initiate verification</b>')
+ pass
+
+ @pyqtSlot(bool)
+ def programDone(self, success):
+ if success:
+ self.statusLabel.setText('Status: <b>Programing complete!</b>')
+ else:
+ self.statusLabel.setText('Status: <b>Programing failed!</b>')
+
+ self.resetButton.setEnabled(True)
+ self.programButton.setEnabled(True)
+ self.imagePathBrowseButton.setEnabled(True)
+
+ @pyqtSlot()
+ def verifyDone(self):
+ self.statusLabel.setText('Status: <b>Verification complete</b>')
+ pass
+
+ @pyqtSlot(str, int)
+ def statusUpdate(self, status, progress):
+ logger.debug("Status: [%s] | %d", status, progress)
+ self.statusLabel.setText('Status: <b>' + status + '</b>')
+ if progress >= 0:
+ self.progressBar.setValue(progress)
+
+ def initiateColdboot(self):
+ self.clt.initiateColdBootSignal.emit("radio://0/100")
+
+
+# No run method specified here as the default run implementation is running the
+# event loop which is what we want
+class CrazyloadThread(QThread):
+ # Input signals declaration (not sure it should be used like that...)
+ program = pyqtSignal(str, bool)
+ verify = pyqtSignal()
+ initiateColdBootSignal = pyqtSignal(str)
+ resetCopterSignal = pyqtSignal()
+ writeConfigSignal = pyqtSignal(int, int, float, float)
+ # Output signals declaration
+ programmed = pyqtSignal(bool)
+ verified = pyqtSignal()
+ statusChanged = pyqtSignal(str, int)
+ connectedSignal = pyqtSignal()
+ connectingSignal = pyqtSignal()
+ failed_signal = pyqtSignal(str)
+ disconnectedSignal = pyqtSignal()
+ updateConfigSignal = pyqtSignal(int, int, float, float)
+ updateCpuIdSignal = pyqtSignal(str)
+
+ radioSpeedPos = 2
+
+ def __init__(self):
+ super(CrazyloadThread, self).__init__()
+
+ self._bl = Bootloader()
+ self._bl.progress_cb = self.statusChanged.emit
+
+ # Make sure that the signals are handled by this thread event loop
+ self.moveToThread(self)
+
+ self.program.connect(self.programAction)
+ self.initiateColdBootSignal.connect(self.initiateColdBoot)
+ self.resetCopterSignal.connect(self.resetCopter)
+
+ def __del__(self):
+ self.quit()
+ self.wait()
+
+ def initiateColdBoot(self, linkURI):
+ self.connectingSignal.emit()
+
+ try:
+ success = self._bl.start_bootloader(warm_boot=False)
+ if not success:
+ self.failed_signal.emit("Could not connect to bootloader")
+ else:
+ self.connectedSignal.emit()
+ except Exception as e:
+ self.failed_signal.emit("{}".format(e))
+
+ def programAction(self, filename, verify):
+ targets = {}
+ if str(filename).endswith("bin"):
+ targets["stm32"] = ("fw", )
+ try:
+ self._bl.flash(str(filename), targets)
+ self.programmed.emit(True)
+ except Exception:
+ self.programmed.emit(False)
+
+ def resetCopter(self):
+ try:
+ self._bl.reset_to_firmware()
+ except Exception:
+ pass
+ self._bl.close()
+ self.disconnectedSignal.emit()
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..4acfd27a8b9a4ed140627350fa9c21415047d163
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.ui
new file mode 100755
index 0000000000000000000000000000000000000000..1254c2970db87e0f20d93caa17b96c695151fc9c
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/bootloader.ui
@@ -0,0 +1,216 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>825</width>
+ <height>338</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Crazyflie Service</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>Crazyflie connection</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="coldBootButton">
+ <property name="text">
+ <string>Initiate bootloader cold boot</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButton_2">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Restart in bootloader mode</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="resetButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Restart in firmware mode</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="_cancel_bootloading">
+ <property name="text">
+ <string>Cancel bootloading</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <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>
+ </item>
+ <item>
+ <widget class="QLabel" name="connectionStatus">
+ <property name="text">
+ <string>Status: <b>Not connected</b></string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="horizontalSpacing">
+ <number>6</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="title">
+ <string>Crazyflie firmware update</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="1" column="2">
+ <widget class="QPushButton" name="imagePathBrowseButton">
+ <property name="text">
+ <string>Browse</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="verifyCheckBox">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Verify flashing</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Select package to flash:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="imagePathLine"/>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <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="verifyButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Verify</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="programButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Program</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <item>
+ <widget class="QProgressBar" name="progressBar">
+ <property name="value">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="statusLabel">
+ <property name="text">
+ <string>Status: <b>IDLE</b></string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.py
new file mode 100755
index 0000000000000000000000000000000000000000..c7635676dacb354115fc7defe26a334839d08172
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.py
@@ -0,0 +1,260 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The bootloader dialog is used to update the Crazyflie firmware and to
+read/write the configuration block in the Crazyflie flash.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Cf1ConfigDialog']
+
+import struct
+import sys
+from cflib.bootloader import Bootloader
+
+import logging
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cfclient.utils.config import Config
+
+service_dialog_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/dialogs/cf1config.ui")[0]
+
+class UIState:
+ DISCONNECTED = 0
+ CONNECTING = 5
+ CONNECT_FAILED = 1
+ COLD_CONNECT = 2
+ FLASHING = 3
+ RESET = 4
+
+
+class Cf1ConfigDialog(QtGui.QWidget, service_dialog_class):
+ """Tab for update the Crazyflie firmware and for reading/writing the config
+ block in flash"""
+ def __init__(self, helper, *args):
+ super(Cf1ConfigDialog, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "CF1 Config"
+ self.menuName = "CF1 Config"
+
+ # self.tabWidget = tabWidget
+ self.helper = helper
+
+ # self.cf = crazyflie
+ self.clt = CrazyloadThread()
+
+ # Connecting GUI signals (a pity to do that manually...)
+ self.coldBootButton.clicked.connect(self.initiateColdboot)
+ self.resetButton.clicked.connect(self.resetCopter)
+ self.saveConfigblock.clicked.connect(self.writeConfig)
+ self._cancel_bootloading.clicked.connect(self.close)
+
+ self.clt.statusChanged.connect(self.statusUpdate)
+ self.clt.connectingSignal.connect(lambda:
+ self.setUiState(UIState.CONNECTING))
+ self.clt.connectedSignal.connect(lambda:
+ self.setUiState(UIState.COLD_CONNECT))
+ self.clt.failed_signal.connect(lambda m: self._ui_connection_fail(m))
+ self.clt.disconnectedSignal.connect(lambda:
+ self.setUiState(UIState.DISCONNECTED))
+ self.clt.updateConfigSignal.connect(self.updateConfig)
+
+ self.clt.start()
+
+ def _ui_connection_fail(self, message):
+ self.setStatusLabel(message)
+ self.coldBootButton.setEnabled(True)
+
+ def setUiState(self, state):
+ if (state == UIState.DISCONNECTED):
+ self.resetButton.setEnabled(False)
+ self.setStatusLabel("Not connected")
+ self.coldBootButton.setEnabled(True)
+ self.progressBar.setTextVisible(False)
+ self.progressBar.setValue(0)
+ self.statusLabel.setText('Status: <b>IDLE</b>')
+ self.saveConfigblock.setEnabled(False)
+ elif (state == UIState.CONNECTING):
+ self.resetButton.setEnabled(False)
+ self.setStatusLabel("Trying to connect cold bootloader, restart "
+ "the Crazyflie to connect")
+ self.coldBootButton.setEnabled(False)
+ elif (state == UIState.CONNECT_FAILED):
+ self.setStatusLabel("Connecting to bootloader failed")
+ self.coldBootButton.setEnabled(True)
+ elif (state == UIState.COLD_CONNECT):
+ self.resetButton.setEnabled(True)
+ self.saveConfigblock.setEnabled(True)
+ self.setStatusLabel("Connected to bootloader")
+ self.coldBootButton.setEnabled(False)
+ elif (state == UIState.RESET):
+ self.setStatusLabel("Resetting to firmware, disconnected")
+ self.resetButton.setEnabled(False)
+ self.coldBootButton.setEnabled(False)
+ self.rollTrim.setValue(0)
+ self.pitchTrim.setValue(0)
+ self.radioChannel.setValue(0)
+ self.radioSpeed.setCurrentIndex(0)
+
+ def setStatusLabel(self, text):
+ self.connectionStatus.setText("Status: <b>%s</b>" % text)
+
+ def connected(self):
+ self.setUiState(UIState.COLD_CONNECT)
+
+ def connectionFailed(self):
+ self.setUiState(UIState.CONNECT_FAILED)
+
+ def resetCopter(self):
+ self.clt.resetCopterSignal.emit()
+ self.setUiState(UIState.RESET)
+
+ def updateConfig(self, channel, speed, rollTrim, pitchTrim):
+ self.rollTrim.setValue(rollTrim)
+ self.pitchTrim.setValue(pitchTrim)
+ self.radioChannel.setValue(channel)
+ self.radioSpeed.setCurrentIndex(speed)
+
+ def closeEvent(self, event):
+ self.setUiState(UIState.RESET)
+ self.clt.resetCopterSignal.emit()
+
+ @pyqtSlot(str, int)
+ def statusUpdate(self, status, progress):
+ logger.debug("Status: [%s] | %d", status, progress)
+ self.statusLabel.setText('Status: <b>' + status + '</b>')
+ if progress >= 0 and progress < 100:
+ self.progressBar.setTextVisible(True)
+ self.progressBar.setValue(progress)
+ else:
+ self.progressBar.setTextVisible(False)
+ self.progressBar.setValue(100)
+ if progress >= 100:
+ self.resetButton.setEnabled(True)
+
+ def initiateColdboot(self):
+ self.clt.initiateColdBootSignal.emit("radio://0/100")
+
+ def writeConfig(self):
+ pitchTrim = self.pitchTrim.value()
+ rollTrim = self.rollTrim.value()
+ channel = self.radioChannel.value()
+ speed = self.radioSpeed.currentIndex()
+
+ self.clt.writeConfigSignal.emit(channel, speed, rollTrim, pitchTrim)
+
+
+# No run method specified here as the default run implementation is running the
+# event loop which is what we want
+class CrazyloadThread(QThread):
+ # Input signals declaration (not sure it should be used like that...)
+ initiateColdBootSignal = pyqtSignal(str)
+ resetCopterSignal = pyqtSignal()
+ writeConfigSignal = pyqtSignal(int, int, float, float)
+ # Output signals declaration
+ statusChanged = pyqtSignal(str, int)
+ connectedSignal = pyqtSignal()
+ connectingSignal = pyqtSignal()
+ failed_signal = pyqtSignal(str)
+ disconnectedSignal = pyqtSignal()
+ updateConfigSignal = pyqtSignal(int, int, float, float)
+
+ def __init__(self):
+ super(CrazyloadThread, self).__init__()
+
+ # Make sure that the signals are handled by this thread event loop
+ self.moveToThread(self)
+ self._bl = Bootloader()
+ self._bl.progress_cb = self.statusChanged.emit
+
+ self.writeConfigSignal.connect(self.writeConfigAction)
+ self.initiateColdBootSignal.connect(self.initiateColdBoot)
+ self.resetCopterSignal.connect(self.resetCopter)
+
+ def __del__(self):
+ self.quit()
+ self.wait()
+
+ def initiateColdBoot(self, linkURI):
+ self.connectingSignal.emit()
+
+ try:
+ success = self._bl.start_bootloader(warm_boot=False)
+ if not success:
+ self.failed_signal.emit("Could not connect to bootloader")
+ else:
+ self.connectedSignal.emit()
+ self.readConfigAction()
+ except Exception as e:
+ self.failed_signal.emit("{}".format(e))
+
+ def checksum256(self, st):
+ return reduce(lambda x, y: x + y, map(ord, st)) % 256
+
+ def writeConfigAction(self, channel, speed, rollTrim, pitchTrim):
+ data = (0x00, channel, speed, pitchTrim, rollTrim)
+ image = struct.pack("<BBBff", *data)
+ # Adding some magic:
+ image = "0xBC" + image
+ image += struct.pack("B", 256 - self.checksum256(image))
+
+ self._bl.write_cf1_config(image)
+
+ def readConfigAction(self):
+ self.statusChanged.emit("Reading config block...", 0)
+ data = self._bl.read_cf1_config()
+ if (data is not None):
+ if data[0:4] == "0xBC":
+ # Skip 0xBC and version at the beginning
+ [channel,
+ speed,
+ pitchTrim,
+ rollTrim] = struct.unpack("<BBff", data[5:15])
+ self.statusChanged.emit("Reading config block...done!", 100)
+ else:
+ channel = Config().get("default_cf_channel")
+ speed = Config().get("default_cf_speed")
+ pitchTrim = Config().get("default_cf_trim")
+ rollTrim = Config().get("default_cf_trim")
+ self.statusChanged.emit("Could not find config block, showing defaults", 100)
+ self.updateConfigSignal.emit(channel, speed, rollTrim, pitchTrim)
+ else:
+ self.statusChanged.emit("Reading config block failed!", 0)
+
+ def resetCopter(self):
+ try:
+ self._bl.reset_to_firmware()
+ except Exception:
+ pass
+ self._bl.close()
+ self.disconnectedSignal.emit()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..ab6c2681b1fd88b2195c184b6971087ffc8feb73
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.ui
new file mode 100755
index 0000000000000000000000000000000000000000..4d7804bb9de6af6a6d1b9997b06b1c2716785088
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf1config.ui
@@ -0,0 +1,260 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>825</width>
+ <height>395</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Crazyflie 1.0 Config</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>Crazyflie connection</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="coldBootButton">
+ <property name="text">
+ <string>Initiate bootloader cold boot</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButton_2">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Restart in bootloader mode</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="resetButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Restart in firmware mode</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="_cancel_bootloading">
+ <property name="text">
+ <string>Cancel bootloading</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <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>
+ </item>
+ <item>
+ <widget class="QLabel" name="connectionStatus">
+ <property name="text">
+ <string>Status: <b>Not connected</b></string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="horizontalSpacing">
+ <number>6</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="configBlockGroup">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="title">
+ <string>Configuration block</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="6" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Radio bandwidth:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QComboBox" name="radioSpeed">
+ <property name="editable">
+ <bool>false</bool>
+ </property>
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <item>
+ <property name="text">
+ <string>250 Kbit/s</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>1 MBit/s</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string> 2 MBit/s</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Pitch trim:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Roll trim:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="2">
+ <widget class="QPushButton" name="saveConfigblock">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Program</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Radio channel:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>0</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="5" column="2">
+ <widget class="QSpinBox" name="radioChannel">
+ <property name="prefix">
+ <string>Ch. </string>
+ </property>
+ <property name="maximum">
+ <number>127</number>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="2">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="2">
+ <widget class="QDoubleSpinBox" name="pitchTrim">
+ <property name="decimals">
+ <number>1</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QDoubleSpinBox" name="rollTrim">
+ <property name="decimals">
+ <number>1</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <item>
+ <widget class="QProgressBar" name="progressBar">
+ <property name="value">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="statusLabel">
+ <property name="text">
+ <string>Status: <b>IDLE</b></string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.py
new file mode 100755
index 0000000000000000000000000000000000000000..236fb2459fd6553d8fc1a8acf066f8c5085af545
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The bootloader dialog is used to update the Crazyflie firmware and to
+read/write the configuration block in the Crazyflie flash.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['CfConfig']
+
+import sys
+import logging
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+from cflib.crazyflie.mem import MemoryElement
+
+service_dialog_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/dialogs/cf2config.ui")[0]
+
+class Cf2ConfigDialog(QtGui.QWidget, service_dialog_class):
+ """Tab for update the Crazyflie firmware and for reading/writing the config
+ block in flash"""
+
+ connected_signal = pyqtSignal(str)
+ disconnected_signal = pyqtSignal(str)
+
+ def __init__(self, helper, *args):
+ super(Cf2ConfigDialog, self).__init__(*args)
+ self.setupUi(self)
+
+ self._cf = helper.cf
+
+ self.disconnected_signal.connect(self._set_ui_disconnected)
+ self.connected_signal.connect(self._set_ui_connected)
+ self._cf.disconnected.add_callback(self.disconnected_signal.emit)
+ self._cf.connected.add_callback(self.connected_signal.emit)
+
+ self._exit_btn.clicked.connect(self.hide)
+ self._write_data_btn.clicked.connect(self._write_data)
+
+ def _write_done(self, mem, addr):
+ self._cf.mem.get_mems(MemoryElement.TYPE_I2C)[0].update(self._data_updated)
+
+ def _data_updated(self, mem):
+ self._roll_trim.setValue(mem.elements["roll_trim"])
+ self._pitch_trim.setValue(mem.elements["pitch_trim"])
+ self._radio_channel.setValue(mem.elements["radio_channel"])
+ self._radio_speed.setCurrentIndex(mem.elements["radio_speed"])
+ if "radio_address" in mem.elements:
+ self._radio_address.setValue(mem.elements["radio_address"])
+ self._radio_address.setEnabled(True)
+ else:
+ self._radio_address.setValue(int("0xE7E7E7E7E7", 0))
+ self._radio_address.setEnabled(False)
+ self._write_data_btn.setEnabled(True)
+
+ def _set_ui_connected(self, link_uri):
+ mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C)
+ if len(mems) > 0:
+ mems[0].update(self._data_updated)
+
+ def _set_ui_disconnected(self, link_uri):
+ self._write_data_btn.setEnabled(False)
+ self._roll_trim.setValue(0)
+ self._pitch_trim.setValue(0)
+ self._radio_channel.setValue(0)
+ self._radio_speed.setCurrentIndex(0)
+ self._radio_address.setValue(0)
+ self._radio_address.setEnabled(False)
+
+ def _write_data(self):
+ self._write_data_btn.setEnabled(False)
+ mem = self._cf.mem.get_mems(MemoryElement.TYPE_I2C)[0]
+ mem.elements["pitch_trim"] = self._pitch_trim.value()
+ mem.elements["roll_trim"] = self._roll_trim.value()
+ mem.elements["radio_channel"] = self._radio_channel.value()
+ mem.elements["radio_speed"] = self._radio_speed.currentIndex()
+ if "radio_address" in mem.elements:
+ mem.elements["radio_address"] = self._radio_address.value()
+ mem.write_data(self._write_done)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..ad60a1c0724d08baa3898ba4db38cf7d0467ec8c
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.ui
new file mode 100755
index 0000000000000000000000000000000000000000..3dfa261ea949adad3969879314de780030390a1f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/cf2config.ui
@@ -0,0 +1,191 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>431</width>
+ <height>258</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Crazyflie 2.0 config</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="horizontalSpacing">
+ <number>6</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="configBlockGroup">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="title">
+ <string>Restart needed for changes to take effect</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="6" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Radio bandwith:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Radio channel:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QSpinBox" name="_radio_channel">
+ <property name="prefix">
+ <string>Ch. </string>
+ </property>
+ <property name="maximum">
+ <number>127</number>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="1">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="_roll_trim">
+ <property name="decimals">
+ <number>1</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="1">
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="_exit_btn">
+ <property name="text">
+ <string>Exit</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="_write_data_btn">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Write</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="_pitch_trim">
+ <property name="decimals">
+ <number>1</number>
+ </property>
+ <property name="minimum">
+ <double>-10.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QComboBox" name="_radio_speed">
+ <property name="editable">
+ <bool>false</bool>
+ </property>
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <item>
+ <property name="text">
+ <string>250 Kbit/s</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>1 MBit/s</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string> 2 MBit/s</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Pitch trim:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Roll trim:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Radio Address:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="HexSpinBox" name="_radio_address">
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <customwidgets>
+ <customwidget>
+ <class>HexSpinBox</class>
+ <extends>QSpinBox</extends>
+ <header>cfclient.ui.widgets.hexspinbox</header>
+ </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.py
new file mode 100755
index 0000000000000000000000000000000000000000..dff76f7bf4f16646f62a80110bb8029aa8e4dbee
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Dialogue that lists available Crazyflies, lets user choose which to connect to.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ConnectionDialogue']
+
+import sys
+
+from PyQt4 import QtGui, uic
+from PyQt4.QtCore import pyqtSignal, pyqtSlot, QThread
+
+import cflib.crtp
+
+(connect_widget_class,
+connect_widget_base_class) = (uic.loadUiType(sys.path[0] +
+ '/cfclient/ui/dialogs/connectiondialogue.ui'))
+
+
+class ConnectDialogue(QtGui.QWidget, connect_widget_class):
+
+ # Outgoing signal for connecting a Crazyflie
+ requestConnectionSignal = pyqtSignal(str)
+
+ def __init__(self, *args):
+ super(ConnectDialogue, self).__init__(*args)
+ self.setupUi(self)
+
+ self.scanner = ScannerThread()
+ self.scanner.start()
+
+ # Connect signals to slots
+ self.connectButton.clicked.connect(self.openConnection)
+ self.scanButton.clicked.connect(self.rescan)
+ self.cancelButton.clicked.connect(self.cancel)
+ self.interfaceList.itemDoubleClicked.connect(self.interfaceSelected)
+ self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
+ self.box = None
+ self.address.setValue(0xE7E7E7E7E7)
+
+ self.available_interfaces = []
+
+ def rescan(self):
+ """Disable all buttons and scan signals from Crazyflies."""
+ self.interfaceList.clear()
+ self.interfaceList.addItem("Scanning...")
+ self.scanButton.setEnabled(False)
+ self.cancelButton.setEnabled(False)
+ self.connectButton.setEnabled(False)
+ self.scanner.scanSignal.emit(self.address.value())
+
+ def foundInterfaces(self, interfaces):
+ """
+ Add found interfaces to list and enable buttons in UI.
+ """
+ self.interfaceList.clear()
+ self.available_interfaces = interfaces
+ for i in interfaces:
+ if (len(i[1]) > 0):
+ self.interfaceList.addItem("%s - %s" % (i[0], i[1]))
+ else:
+ self.interfaceList.addItem(i[0])
+ if len(interfaces) > 0:
+ self.interfaceList.setCurrentRow(0)
+ self.connectButton.setEnabled(True)
+ self.cancelButton.setEnabled(True)
+ self.scanButton.setEnabled(True)
+
+ def interfaceSelected(self, listItem):
+ self.requestConnectionSignal.emit(
+ self.available_interfaces[self.interfaceList.currentRow()][0])
+ self.close()
+
+ def openConnection(self):
+ self.interfaceSelected(self.interfaceList.currentItem())
+
+ def cancel(self):
+ self.close()
+
+ def showEvent(self, ev):
+ self.rescan()
+
+
+class ScannerThread(QThread):
+
+ scanSignal = pyqtSignal(object)
+ interfaceFoundSignal = pyqtSignal(object)
+
+ def __init__(self):
+ QThread.__init__(self)
+ self.moveToThread(self)
+ self.scanSignal.connect(self.scan)
+
+ def scan(self, address):
+ self.interfaceFoundSignal.emit(cflib.crtp.scan_interfaces(address))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..cbacbabb4c9977ebb812ac2b985d07e1c9071eaf
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.ui
new file mode 100755
index 0000000000000000000000000000000000000000..3e1f25aa0553496ae738d1373c1a7f12929ef49d
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/connectiondialogue.ui
@@ -0,0 +1,102 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>551</width>
+ <height>350</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Connect Crazyflie</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Connect to Crazyflie</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QListWidget" name="interfaceList"/>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Address:</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="HexSpinBox" name="address">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="scanButton">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Scan</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="connectButton">
+ <property name="text">
+ <string>Connect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="cancelButton">
+ <property name="text">
+ <string>Cancel</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <customwidgets>
+ <customwidget>
+ <class>HexSpinBox</class>
+ <extends>QSpinBox</extends>
+ <header>cfclient.ui.widgets.hexspinbox</header>
+ </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.py
new file mode 100755
index 0000000000000000000000000000000000000000..ec8ed5ac0fad262564c69d19e5d9634e3b2f06bd
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.py
@@ -0,0 +1,423 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Dialogue used to select and configure an inputdevice. This includes mapping buttons and
+axis to match controls for the Crazyflie.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['InputConfigDialogue']
+
+import sys
+import json
+import logging
+
+logger = logging.getLogger(__name__)
+
+from cfclient.utils.config_manager import ConfigManager
+from cflib.crtp.exceptions import CommunicationException
+
+from PyQt4 import Qt, QtCore, QtGui, uic
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+from PyQt4.Qt import *
+
+from cfclient.utils.input import JoystickReader
+
+(inputconfig_widget_class,
+connect_widget_base_class) = (uic.loadUiType(sys.path[0] +
+ '/cfclient/ui/dialogs/inputconfigdialogue.ui'))
+
+
+class InputConfigDialogue(QtGui.QWidget, inputconfig_widget_class):
+
+ def __init__(self, joystickReader, *args):
+ super(InputConfigDialogue, self).__init__(*args)
+ self.setupUi(self)
+ self._input = joystickReader
+
+ self._input_device_reader = DeviceReader(self._input)
+ self._input_device_reader.start()
+
+ self._input_device_reader.raw_axis_data_signal.connect(self._detect_axis)
+ self._input_device_reader.raw_button_data_signal.connect(self._detect_button)
+ self._input_device_reader.mapped_values_signal.connect(self._update_mapped_values)
+
+ self.cancelButton.clicked.connect(self.close)
+ self.saveButton.clicked.connect(self._save_config)
+
+ self.detectPitch.clicked.connect(lambda : self._axis_detect("pitch", "Pitch axis",
+ "Center the pitch axis then do max %s pitch", ["forward", "backward"]))
+ self.detectRoll.clicked.connect(lambda : self._axis_detect("roll", "Roll axis",
+ "Center the roll axis and then do max %s roll", ["right", "left"]))
+ self.detectYaw.clicked.connect(lambda : self._axis_detect("yaw", "Yaw axis",
+ "Center the yaw axis and then do max %s yaw", ["right", "left"]))
+ self.detectThrust.clicked.connect(lambda : self._axis_detect("thrust", "Thrust axis",
+ "Center the thrust axis, and then do max thrust (also used to adjust target altitude in altitude hold mode)"))
+ self.detectPitchPos.clicked.connect(lambda : self._button_detect("pitchPos", "Pitch Cal Positive",
+ "Press the button for Pitch postive calibration"))
+ self.detectPitchNeg.clicked.connect(lambda : self._button_detect("pitchNeg", "Pitch Cal Negative",
+ "Press the button for Pitch negative calibration"))
+ self.detectRollPos.clicked.connect(lambda : self._button_detect("rollPos", "Roll Cal Positive",
+ "Press the button for Roll positive calibration"))
+ self.detectRollNeg.clicked.connect(lambda : self._button_detect("rollNeg", "Roll Cal Negative",
+ "Press the button for Roll negative calibration"))
+ self.detectKillswitch.clicked.connect(lambda : self._button_detect("killswitch", "Killswitch",
+ "Press the button for the killswitch (will disable motors)"))
+ self.detectAlt1.clicked.connect(lambda : self._button_detect("alt1", "Alternative function 1",
+ "The alternative function 1 that will do a callback"))
+ self.detectAlt2.clicked.connect(lambda : self._button_detect("alt2", "Alternative function 2",
+ "The alternative function 2 that will do a callback"))
+ self.detectExitapp.clicked.connect(lambda : self._button_detect("exitapp", "Exit application",
+ "Press the button for exiting the application"))
+ self.detectAltHold.clicked.connect(lambda : self._button_detect("althold", "Altitude hold",
+ "Press the button for altitude hold mode activation (releasing returns to manual mode)"))
+ self.detectMuxswitch.clicked.connect(lambda: self._button_detect("muxswitch", "Mux Switch",
+ "Press the button for mux switching"))
+
+
+ self.configButton.clicked.connect(self._start_configuration)
+ self.loadButton.clicked.connect(self._load_config_from_file)
+ self.deleteButton.clicked.connect(self._delete_configuration)
+
+ self._popup = None
+ self._combined_button = None
+ self._detection_buttons = [self.detectPitch, self.detectRoll,
+ self.detectYaw, self.detectThrust,
+ self.detectPitchPos, self.detectPitchNeg,
+ self.detectRollPos, self.detectRollNeg,
+ self.detectKillswitch, self.detectExitapp,
+ self.detectAltHold, self.detectAlt1,
+ self.detectAlt2, self.detectMuxswitch]
+
+ self._button_to_detect = ""
+ self._axis_to_detect = ""
+ self.combinedDetection = 0
+ self._prev_combined_id = None
+
+ self._maxed_axis = []
+ self._mined_axis = []
+
+ self._buttonindicators = {}
+ self._axisindicators = {}
+ self._reset_mapping()
+
+ for d in self._input.available_devices():
+ if d.supports_mapping:
+ self.inputDeviceSelector.addItem(d.name, d.id)
+
+ if len(self._input.available_devices()) > 0:
+ self.configButton.setEnabled(True)
+
+ self._map = {}
+ self._saved_open_device = None
+
+ @staticmethod
+ def _scale(max_value, value):
+ return (value/max_value) * 100
+
+ def _reset_mapping(self):
+ self._buttonindicators= {
+ "pitchPos": self.pitchPos,
+ "pitchNeg": self.pitchNeg,
+ "rollPos": self.rollPos,
+ "rollNeg": self.rollNeg,
+ "killswitch": self.killswitch,
+ "alt1": self.alt1,
+ "alt2": self.alt2,
+ "exitapp": self.exitapp,
+ "althold": self.althold,
+ "muxswitch": self.muxswitch,
+ }
+
+ self._axisindicators = {
+ "pitch": self.pitchAxisValue,
+ "roll": self.rollAxisValue,
+ "yaw": self.yawAxisValue,
+ "thrust": self.thrustAxisValue,
+ }
+
+ def _cancel_config_popup(self, button):
+ self._axis_to_detect = ""
+ self._button_to_detect = ""
+
+ def _show_config_popup(self, caption, message, directions=[]):
+ self._maxed_axis = []
+ self._mined_axis = []
+ self._popup = QMessageBox()
+ self._popup.directions = directions
+ self._combined_button = QtGui.QPushButton('Combined Axis Detection')
+ self.cancelButton = QtGui.QPushButton('Cancel')
+ self._popup.addButton(self.cancelButton, QMessageBox.DestructiveRole)
+ self._popup.setWindowTitle(caption)
+ self._popup.setWindowFlags(Qt.Dialog|Qt.MSWindowsFixedSizeDialogHint)
+ if len(directions) > 1:
+ self._popup.originalMessage = message
+ message = self._popup.originalMessage % directions[0]
+ self._combined_button.setCheckable(True)
+ self._combined_button.blockSignals(True)
+ self._popup.addButton(self._combined_button, QMessageBox.ActionRole)
+ self._popup.setText(message)
+ self._popup.show()
+
+ def _start_configuration(self):
+ self._input.enableRawReading(str(self.inputDeviceSelector.currentText()))
+ self._input_device_reader.start_reading()
+ self._populate_config_dropdown()
+ self.profileCombo.setEnabled(True)
+ for b in self._detection_buttons:
+ b.setEnabled(True)
+
+ def _detect_axis(self, data):
+ if (len(self._axis_to_detect) > 0):
+ if self._combined_button and self._combined_button.isChecked() and self.combinedDetection == 0:
+ self._combined_button.setDisabled(True)
+ self.combinedDetection = 1
+ for a in data:
+ # Axis must go low and high before it's accepted as selected
+ # otherwise maxed out axis (like gyro/acc) in some controllers
+ # will always be selected. Not enforcing negative values makes it
+ # possible to detect split axis (like bumpers on PS3 controller)
+ if a not in self._maxed_axis and abs(data[a]) > 0.8:
+ self._maxed_axis.append(a)
+ if a not in self._mined_axis and abs(data[a]) < 0.1:
+ self._mined_axis.append(a)
+ if a in self._maxed_axis and a in self._mined_axis and len(self._axis_to_detect) > 0:
+ if self.combinedDetection == 0:
+ if data[a] >= 0:
+ self._map_axis(self._axis_to_detect, a, 1.0)
+ else:
+ self._map_axis(self._axis_to_detect, a, -1.0)
+ self._axis_to_detect = ""
+ self._check_and_enable_saving()
+ if self._popup != None:
+ self.cancelButton.click()
+ elif self.combinedDetection == 2: #finished detection
+ if self._prev_combined_id != a: # not the same axis again ...
+ self._map_axis(self._axis_to_detect, a, -1.0)
+ self._axis_to_detect = ""
+ self._check_and_enable_saving()
+ if (self._popup != None):
+ self.cancelButton.click()
+ self.combinedDetection = 0
+ elif self.combinedDetection == 1:
+ self._map_axis(self._axis_to_detect, a, 1.0)
+ self._prev_combined_id = a
+ self.combinedDetection = 2
+ message = self._popup.originalMessage % self._popup.directions[1]
+ self._popup.setText(message)
+
+ def _update_mapped_values(self, mapped_data):
+ for v in mapped_data.get_all_indicators():
+ if v in self._buttonindicators:
+ if mapped_data.get(v):
+ self._buttonindicators[v].setChecked(True)
+ else:
+ self._buttonindicators[v].setChecked(False)
+ if v in self._axisindicators:
+ # The sliders used are set to 0-100 and the values from the
+ # input-layer is scaled according to the max settings in
+ # the input-layer. So scale the value and place 0 in the middle.
+ scaled_value = mapped_data.get(v)
+ if v == "thrust":
+ scaled_value = InputConfigDialogue._scale(
+ self._input.max_thrust, scaled_value
+ )
+ if v == "roll" or v == "pitch":
+ scaled_value = InputConfigDialogue._scale(
+ self._input.max_rp_angle, scaled_value
+ )
+ if v == "yaw":
+ scaled_value = InputConfigDialogue._scale(
+ self._input.max_yaw_rate, scaled_value
+ )
+ self._axisindicators[v].setValue(scaled_value)
+
+ def _map_axis(self, function, key_id, scale):
+ self._map["Input.AXIS-{}".format(key_id)] = {}
+ self._map["Input.AXIS-{}".format(key_id)]["id"] = key_id
+ self._map["Input.AXIS-{}".format(key_id)]["key"] = function
+ self._map["Input.AXIS-{}".format(key_id)]["scale"] = scale
+ self._map["Input.AXIS-{}".format(key_id)]["offset"] = 0.0
+ self._map["Input.AXIS-{}".format(key_id)]["type"] = "Input.AXIS"
+ self._input.set_raw_input_map(self._map)
+
+ def _map_button(self, function, key_id):
+ # Duplicate buttons is not allowed, remove if there's already one mapped
+ prev_button = None
+ for m in self._map:
+ if "key" in self._map[m] and self._map[m]["key"] == function:
+ prev_button = m
+ if prev_button:
+ del self._map[prev_button]
+
+ self._map["Input.BUTTON-{}".format(key_id)] = {}
+ self._map["Input.BUTTON-{}".format(key_id)]["id"] = key_id
+ self._map["Input.BUTTON-{}".format(key_id)]["key"] = function
+ self._map["Input.BUTTON-{}".format(key_id)]["scale"] = 1.0
+ self._map["Input.BUTTON-{}".format(key_id)]["type"] = "Input.BUTTON"
+ self._input.set_raw_input_map(self._map)
+
+ def _detect_button(self, data):
+ if len(self._button_to_detect) > 0:
+ for b in data:
+ if data[b] > 0:
+ self._map_button(self._button_to_detect, b)
+ self._button_to_detect = ""
+ self._check_and_enable_saving()
+ if self._popup != None:
+ self._popup.close()
+
+ def _check_and_enable_saving(self):
+ needed_funcs = ["thrust", "yaw", "roll", "pitch"]
+
+ for m in self._map:
+ if self._map[m]["key"] in needed_funcs:
+ needed_funcs.remove(self._map[m]["key"])
+
+ if len(needed_funcs) == 0:
+ self.saveButton.setEnabled(True)
+
+ def _populate_config_dropdown(self):
+ configs = ConfigManager().get_list_of_configs()
+ if len(configs):
+ self.loadButton.setEnabled(True)
+ for c in configs:
+ self.profileCombo.addItem(c)
+
+ def _axis_detect(self, varname, caption, message, directions=[]):
+ self._axis_to_detect = varname
+ self._show_config_popup(caption, message, directions)
+
+ def _button_detect(self, varname, caption, message):
+ self._button_to_detect = varname
+ self._show_config_popup(caption, message)
+
+ def _show_error(self, caption, message):
+ QMessageBox.critical(self, caption, message)
+
+ def _load_config_from_file(self):
+ loaded_map = ConfigManager().get_config(self.profileCombo.currentText())
+ if loaded_map:
+ self._input.set_raw_input_map(loaded_map)
+ self._map = loaded_map
+ else:
+ logger.warning("Could not load configfile [%s]",
+ self.profileCombo.currentText())
+ self._show_error("Could not load config",
+ "Could not load config [%s]" %
+ self.profileCombo.currentText())
+ self._check_and_enable_saving()
+
+ def _delete_configuration(self):
+ logger.warning("deleteConfig not implemented")
+
+ def _save_config(self):
+ configName = str(self.profileCombo.currentText())
+
+ mapping = {'inputconfig': {'inputdevice': {'axis': []}}}
+
+ # Create intermediate structure for the configuration file
+ funcs = {}
+ for m in self._map:
+ key = self._map[m]["key"]
+ if not key in funcs:
+ funcs[key] = []
+ funcs[key].append(self._map[m])
+
+ # Create a mapping for each axis, take care to handle
+ # split axis configurations
+ for a in funcs:
+ func = funcs[a]
+ axis = {}
+ # Check for split axis
+ if len(func) > 1:
+ axis["ids"] = [func[0]["id"], func[1]["id"]]
+ axis["scale"] = func[1]["scale"]
+ else:
+ axis["id"] = func[0]["id"]
+ axis["scale"] = func[0]["scale"]
+ axis["key"] = func[0]["key"]
+ axis["name"] = func[0]["key"] # Name isn't used...
+ axis["type"] =func[0]["type"]
+ mapping["inputconfig"]["inputdevice"]["axis"].append(axis)
+
+ mapping["inputconfig"]['inputdevice']['name'] = configName
+ mapping["inputconfig"]['inputdevice']['updateperiod'] = 10
+
+ config_name = self.profileCombo.currentText()
+ filename = ConfigManager().configs_dir + "/%s.json" % config_name
+ logger.info("Saving config to [%s]", filename)
+ json_data = open(filename, 'w')
+ json_data.write(json.dumps(mapping, indent=2))
+ json_data.close()
+
+ ConfigManager().conf_needs_reload.call(config_name)
+ self.close()
+
+ def showEvent(self, event):
+ """Called when dialog is opened"""
+ #self._saved_open_device = self._input.get_device_name()
+ #self._input.stop_input()
+ self._input.pause_input()
+
+ def closeEvent(self, event):
+ """Called when dialog is closed"""
+ self._input.stop_raw_reading()
+ self._input_device_reader.stop_reading()
+ #self._input.start_input(self._saved_open_device)
+ self._input.resume_input()
+
+class DeviceReader(QThread):
+ """Used for polling data from the Input layer during configuration"""
+ raw_axis_data_signal = pyqtSignal(object)
+ raw_button_data_signal = pyqtSignal(object)
+ mapped_values_signal = pyqtSignal(object)
+
+ def __init__(self, input):
+ QThread.__init__(self)
+
+ self._input = input
+ self._read_timer = QTimer()
+ self._read_timer.setInterval(25)
+
+ self.connect(self._read_timer, SIGNAL("timeout()"), self._read_input)
+
+ def stop_reading(self):
+ """Stop polling data"""
+ self._read_timer.stop()
+
+ def start_reading(self):
+ """Start polling data"""
+ self._read_timer.start()
+
+ def _read_input(self):
+ [rawaxis, rawbuttons, mapped_values] = self._input.read_raw_values()
+ self.raw_axis_data_signal.emit(rawaxis)
+ self.raw_button_data_signal.emit(rawbuttons)
+ self.mapped_values_signal.emit(mapped_values)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..0b7a51788ba0f7690c7604f11af4892f6bb744af
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.ui
new file mode 100755
index 0000000000000000000000000000000000000000..a8032bbde8cfc60601db9cfaebd3cc11b41276c9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/inputconfigdialogue.ui
@@ -0,0 +1,753 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>957</width>
+ <height>512</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Input device configuration</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Configure input device</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Input device</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QComboBox" name="inputDeviceSelector">
+ <property name="minimumSize">
+ <size>
+ <width>700</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="duplicatesEnabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="configButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Configure</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="inputDetectionLayout">
+ <item>
+ <widget class="QGroupBox" name="inputDetectionGroup">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>350</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>500</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>Input response</string>
+ </property>
+ <widget class="QWidget" name="gridLayoutWidget">
+ <property name="geometry">
+ <rect>
+ <x>-1</x>
+ <y>19</y>
+ <width>301</width>
+ <height>141</height>
+ </rect>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="1">
+ <widget class="QSlider" name="rollAxisValue">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="minimum">
+ <number>-100</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="sliderPosition">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::NoTicks</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Pitch</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Roll</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSlider" name="pitchAxisValue">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="minimum">
+ <number>-100</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="singleStep">
+ <number>1</number>
+ </property>
+ <property name="sliderPosition">
+ <number>0</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::NoTicks</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="detectRoll">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="detectPitch">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="gridLayoutWidget_4">
+ <property name="geometry">
+ <rect>
+ <x>310</x>
+ <y>20</y>
+ <width>301</width>
+ <height>141</height>
+ </rect>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Thrust</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSlider" name="thrustAxisValue">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="minimum">
+ <number>0</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="singleStep">
+ <number>0</number>
+ </property>
+ <property name="sliderPosition">
+ <number>0</number>
+ </property>
+ <property name="tracking">
+ <bool>true</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::NoTicks</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="yawAxisValue">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="minimum">
+ <number>-100</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="singleStep">
+ <number>0</number>
+ </property>
+ <property name="sliderPosition">
+ <number>1</number>
+ </property>
+ <property name="tracking">
+ <bool>true</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::NoTicks</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="detectThrust">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Yaw</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="detectYaw">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="gridLayoutWidget_5">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>170</y>
+ <width>518</width>
+ <height>151</height>
+ </rect>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="2" column="1">
+ <widget class="QCheckBox" name="rollNeg">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Roll -</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QCheckBox" name="pitchNeg">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Pitch -</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QPushButton" name="detectPitchNeg">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QCheckBox" name="rollPos">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Roll +</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QPushButton" name="detectRollPos">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="detectPitchPos">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="detectRollNeg">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QCheckBox" name="pitchPos">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Pitch +</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="gridLayoutWidget_2">
+ <property name="geometry">
+ <rect>
+ <x>600</x>
+ <y>158</y>
+ <width>292</width>
+ <height>194</height>
+ </rect>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="5" column="1">
+ <widget class="QPushButton" name="detectAlt2">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QCheckBox" name="alt1">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Alt 1</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QPushButton" name="detectExitapp">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="detectKillswitch">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="killswitch">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Killswitch</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QCheckBox" name="exitapp">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Exit app</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QCheckBox" name="althold">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Altitude hold</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="detectAltHold">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QCheckBox" name="alt2">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Alt 2</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QPushButton" name="detectAlt1">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QCheckBox" name="muxswitch">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Mux switch</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QPushButton" name="detectMuxswitch">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Detect</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>Profile name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <spacer name="horizontalSpacer_9">
+ <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 row="0" column="4">
+ <widget class="QPushButton" name="deleteButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Delete</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QPushButton" name="cancelButton">
+ <property name="text">
+ <string>Cancel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QComboBox" name="profileCombo">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>400</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="editable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="7">
+ <widget class="QPushButton" name="saveButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Save</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QPushButton" name="loadButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Load</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.py
new file mode 100755
index 0000000000000000000000000000000000000000..31ac80bd100881a566483994b9007661239ded0e
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.py
@@ -0,0 +1,283 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+This dialogue is used to configure different log configurations that is used to
+enable logging of data from the Crazyflie. These can then be used in different
+views in the UI.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogConfigDialogue']
+
+import sys
+import os
+import logging
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import Qt, QtCore, QtGui, uic
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+from PyQt4.Qt import *
+
+from cflib.crazyflie.log import Log, LogVariable, LogConfig
+
+(logconfig_widget_class,
+connect_widget_base_class) = (uic.loadUiType(sys.path[0] +
+ '/cfclient/ui/dialogs/logconfigdialogue.ui'))
+
+NAME_FIELD = 0
+ID_FIELD = 1
+PTYPE_FIELD = 2
+CTYPE_FIELD = 3
+
+
+class LogConfigDialogue(QtGui.QWidget, logconfig_widget_class):
+
+ def __init__(self, helper, *args):
+ super(LogConfigDialogue, self).__init__(*args)
+ self.setupUi(self)
+ self.helper = helper
+
+ self.logTree.setHeaderLabels(['Name', 'ID', 'Unpack', 'Storage'])
+ self.varTree.setHeaderLabels(['Name', 'ID', 'Unpack', 'Storage'])
+
+ self.addButton.clicked.connect(lambda: self.moveNode(self.logTree,
+ self.varTree))
+ self.removeButton.clicked.connect(lambda: self.moveNode(self.varTree,
+ self.logTree))
+ self.cancelButton.clicked.connect(self.close)
+ self.loadButton.clicked.connect(self.loadConfig)
+ self.saveButton.clicked.connect(self.saveConfig)
+
+ self.loggingPeriod.textChanged.connect(self.periodChanged)
+
+ self.packetSize.setMaximum(30)
+ self.currentSize = 0
+ self.packetSize.setValue(0)
+ self.period = 0
+
+ def decodeSize(self, s):
+ size = 0
+ if ("16" in s):
+ size = 2
+ if ("float" in s):
+ size = 4
+ if ("8" in s):
+ size = 1
+ if ("FP16" in s):
+ size = 2
+ if ("32" in s):
+ size = 4
+ return size
+
+ def sortTrees(self):
+ self.varTree.invisibleRootItem().sortChildren(NAME_FIELD,
+ Qt.AscendingOrder)
+ for node in self.getNodeChildren(self.varTree.invisibleRootItem()):
+ node.sortChildren(NAME_FIELD, Qt.AscendingOrder)
+ self.logTree.invisibleRootItem().sortChildren(NAME_FIELD,
+ Qt.AscendingOrder)
+ for node in self.getNodeChildren(self.logTree.invisibleRootItem()):
+ node.sortChildren(NAME_FIELD, Qt.AscendingOrder)
+
+ def getNodeChildren(self, treeNode):
+ children = []
+ for i in range(treeNode.childCount()):
+ children.append(treeNode.child(i))
+ return children
+
+ def updatePacketSizeBar(self):
+ self.currentSize = 0
+ for node in self.getNodeChildren(self.varTree.invisibleRootItem()):
+ for leaf in self.getNodeChildren(node):
+ self.currentSize = (self.currentSize +
+ self.decodeSize(leaf.text(CTYPE_FIELD)))
+ self.packetSize.setValue(self.currentSize)
+
+ def addNewVar(self, logTreeItem, target):
+ parentName = logTreeItem.parent().text(NAME_FIELD)
+ varParent = target.findItems(parentName, Qt.MatchExactly, NAME_FIELD)
+
+ item = logTreeItem.clone()
+
+ if (len(varParent) == 0):
+ newParent = QtGui.QTreeWidgetItem()
+ newParent.setData(0, Qt.DisplayRole, parentName)
+ newParent.addChild(item)
+ target.addTopLevelItem(newParent)
+ target.expandItem(newParent)
+ else:
+ parent = varParent[0]
+ parent.addChild(item)
+
+ def moveNodeItem(self, source, target, item):
+ if (item.parent() == None):
+ children = self.getNodeChildren(item)
+ for c in children:
+ self.addNewVar(c, target)
+ source.takeTopLevelItem(source.indexOfTopLevelItem(item))
+ elif (item.parent().childCount() > 1):
+ self.addNewVar(item, target)
+ item.parent().removeChild(item)
+ else:
+ self.addNewVar(item, target)
+ # item.parent().removeChild(item)
+ source.takeTopLevelItem(source.indexOfTopLevelItem(item.parent()))
+ self.updatePacketSizeBar()
+ self.sortTrees()
+ self.checkAndEnableSaveButton()
+
+ def checkAndEnableSaveButton(self):
+ if (self.currentSize > 0 and self.period > 0):
+ self.saveButton.setEnabled(True)
+ else:
+ self.saveButton.setEnabled(False)
+
+ def moveNode(self, source, target):
+ self.moveNodeItem(source, target, source.currentItem())
+
+ def moveNodeByName(self, source, target, parentName, itemName):
+ parents = source.findItems(parentName, Qt.MatchExactly, NAME_FIELD)
+ node = None
+ if (len(parents) > 0):
+ parent = parents[0]
+ for n in range(parent.childCount()):
+ if (parent.child(n).text(NAME_FIELD) == itemName):
+ node = parent.child(n)
+ break
+ if (node != None):
+ self.moveNodeItem(source, target, node)
+ return True
+ return False
+
+ def showEvent(self, event):
+ self.updateToc()
+ self.populateDropDown()
+ toc = self.helper.cf.log.toc
+ if (len(toc.toc.keys()) > 0):
+ self.configNameCombo.setEnabled(True)
+ else:
+ self.configNameCombo.setEnabled(False)
+
+ def resetTrees(self):
+ self.varTree.clear()
+ self.updateToc()
+
+ def periodChanged(self, value):
+ try:
+ self.period = int(value)
+ self.checkAndEnableSaveButton()
+ except:
+ self.period = 0
+
+ def showErrorPopup(self, caption, message):
+ self.box = QMessageBox()
+ self.box.setWindowTitle(caption)
+ self.box.setText(message)
+ # self.box.setButtonText(1, "Ok")
+ self.box.setWindowFlags(Qt.Dialog | Qt.MSWindowsFixedSizeDialogHint)
+ self.box.show()
+
+ def updateToc(self):
+ self.logTree.clear()
+
+ toc = self.helper.cf.log.toc
+
+ for group in toc.toc.keys():
+ groupItem = QtGui.QTreeWidgetItem()
+ groupItem.setData(NAME_FIELD, Qt.DisplayRole, group)
+ for param in toc.toc[group].keys():
+ item = QtGui.QTreeWidgetItem()
+ item.setData(NAME_FIELD, Qt.DisplayRole, param)
+ item.setData(ID_FIELD, Qt.DisplayRole,
+ toc.toc[group][param].ident)
+ item.setData(PTYPE_FIELD, Qt.DisplayRole,
+ toc.toc[group][param].pytype)
+ item.setData(CTYPE_FIELD, Qt.DisplayRole,
+ toc.toc[group][param].ctype)
+ groupItem.addChild(item)
+
+ self.logTree.addTopLevelItem(groupItem)
+ self.logTree.expandItem(groupItem)
+ self.sortTrees()
+
+ def populateDropDown(self):
+ self.configNameCombo.clear()
+ toc = self.helper.logConfigReader.getLogConfigs()
+ for d in toc:
+ self.configNameCombo.addItem(d.name)
+ if (len(toc) > 0):
+ self.loadButton.setEnabled(True)
+
+ def loadConfig(self):
+ cText = self.configNameCombo.currentText()
+ config = None
+ for d in self.helper.logConfigReader.getLogConfigs():
+ if (d.name == cText):
+ config = d
+ if (config == None):
+ logger.warning("Could not load config")
+ else:
+ self.resetTrees()
+ self.loggingPeriod.setText("%d" % config.period_in_ms)
+ self.period = config.period_in_ms
+ for v in config.variables:
+ if (v.is_toc_variable()):
+ parts = v.name.split(".")
+ varParent = parts[0]
+ varName = parts[1]
+ if self.moveNodeByName(self.logTree,
+ self.varTree,
+ varParent,
+ varName) == False:
+ logger.warning("Could not find node %s.%s!!", varParent, varName)
+ else:
+ logger.warning("Error: Mem vars not supported!")
+
+ def saveConfig(self):
+ updatedConfig = self.createConfigFromSelection()
+ try:
+ self.helper.logConfigReader.saveLogConfigFile(updatedConfig)
+ self.close()
+ except Exception as e:
+ self.showErrorPopup("Error when saving file", "Error: %s" % e)
+ self.helper.cf.log.add_config(updatedConfig)
+
+ def createConfigFromSelection(self):
+ logconfig = LogConfig(str(self.configNameCombo.currentText()),
+ self.period)
+ for node in self.getNodeChildren(self.varTree.invisibleRootItem()):
+ parentName = node.text(NAME_FIELD)
+ for leaf in self.getNodeChildren(node):
+ varName = leaf.text(NAME_FIELD)
+ varType = str(leaf.text(CTYPE_FIELD))
+ completeName = "%s.%s" % (parentName, varName)
+ logconfig.add_variable(completeName, varType)
+ return logconfig
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..b40dc18a485fa00619560986b8b0610fc644e4df
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.ui
new file mode 100755
index 0000000000000000000000000000000000000000..d0cc7228e06294fd0aea87b8c01ba2e5f03983a7
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/dialogs/logconfigdialogue.ui
@@ -0,0 +1,284 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="windowModality">
+ <enum>Qt::ApplicationModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>925</width>
+ <height>447</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Log configuration</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Create and modify log configurations</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="0">
+ <widget class="QTreeWidget" name="logTree">
+ <property name="columnCount">
+ <number>4</number>
+ </property>
+ <attribute name="headerDefaultSectionSize">
+ <number>100</number>
+ </attribute>
+ <column>
+ <property name="text">
+ <string notr="true">1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">2</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">3</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">4</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QPushButton" name="addButton">
+ <property name="text">
+ <string>--></string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="removeButton">
+ <property name="text">
+ <string><--</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="2">
+ <widget class="QTreeWidget" name="varTree">
+ <property name="columnCount">
+ <number>4</number>
+ </property>
+ <attribute name="headerDefaultSectionSize">
+ <number>100</number>
+ </attribute>
+ <column>
+ <property name="text">
+ <string notr="true">1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">2</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">3</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">4</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item alignment="Qt::AlignTop">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Logging period</string>
+ </property>
+ </widget>
+ </item>
+ <item alignment="Qt::AlignTop">
+ <widget class="QLineEdit" name="loggingPeriod">
+ <property name="maximumSize">
+ <size>
+ <width>100</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item alignment="Qt::AlignTop">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>ms</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <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>
+ </item>
+ <item>
+ <widget class="QProgressBar" name="packetSize">
+ <property name="value">
+ <number>24</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>Config name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <spacer name="horizontalSpacer_9">
+ <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 row="0" column="3">
+ <widget class="QPushButton" name="deleteButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Delete</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QPushButton" name="cancelButton">
+ <property name="text">
+ <string>Cancel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QComboBox" name="configNameCombo">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>400</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="editable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QPushButton" name="saveButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Save</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QPushButton" name="loadButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Load</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.py
new file mode 100755
index 0000000000000000000000000000000000000000..0c3b59b2830eb16929b5dd1f87165c4800ee8a25
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.py
@@ -0,0 +1,667 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The main file for the Crazyflie control application.
+"""
+__author__ = 'Bitcraze AB'
+__all__ = ['MainUI']
+
+import sys
+import logging
+
+
+logger = logging.getLogger(__name__)
+
+import PyQt4
+from PyQt4 import QtGui, uic
+from PyQt4.QtCore import pyqtSignal, Qt, pyqtSlot, QDir, QUrl
+from PyQt4.QtGui import QLabel, QActionGroup, QMessageBox, QAction, QDesktopServices, QMenu
+
+from dialogs.connectiondialogue import ConnectDialogue
+from dialogs.inputconfigdialogue import InputConfigDialogue
+from dialogs.cf2config import Cf2ConfigDialog
+from dialogs.cf1config import Cf1ConfigDialog
+from cflib.crazyflie import Crazyflie
+from dialogs.logconfigdialogue import LogConfigDialogue
+
+from cfclient.utils.input import JoystickReader
+from cfclient.utils.zmq_param import ZMQParamAccess
+from cfclient.utils.zmq_led_driver import ZMQLEDDriver
+from cfclient.utils.config import Config
+from cfclient.utils.logconfigreader import LogConfigReader
+from cfclient.utils.config_manager import ConfigManager
+
+import cfclient.ui.toolboxes
+import cfclient.ui.tabs
+import cflib.crtp
+
+from cflib.crazyflie.log import Log, LogVariable, LogConfig
+
+from cfclient.ui.dialogs.bootloader import BootloaderDialog
+from cfclient.ui.dialogs.about import AboutDialog
+
+from cflib.crazyflie.mem import MemoryElement
+
+(main_window_class,
+main_windows_base_class) = (uic.loadUiType(sys.path[0] +
+ '/cfclient/ui/main.ui'))
+
+
+class MyDockWidget(QtGui.QDockWidget):
+ closed = pyqtSignal()
+
+ def closeEvent(self, event):
+ super(MyDockWidget, self).closeEvent(event)
+ self.closed.emit()
+
+
+class UIState:
+ DISCONNECTED = 0
+ CONNECTING = 1
+ CONNECTED = 2
+
+
+class MainUI(QtGui.QMainWindow, main_window_class):
+
+ connectionLostSignal = pyqtSignal(str, str)
+ connectionInitiatedSignal = pyqtSignal(str)
+ batteryUpdatedSignal = pyqtSignal(int, object, object)
+ connectionDoneSignal = pyqtSignal(str)
+ connectionFailedSignal = pyqtSignal(str, str)
+ disconnectedSignal = pyqtSignal(str)
+ linkQualitySignal = pyqtSignal(int)
+
+ _input_device_error_signal = pyqtSignal(str)
+ _input_discovery_signal = pyqtSignal(object)
+ _log_error_signal = pyqtSignal(object, str)
+
+ def __init__(self, *args):
+ super(MainUI, self).__init__(*args)
+ self.setupUi(self)
+
+ ######################################################
+ ### By lxrocks
+ ### 'Skinny Progress Bar' tweak for Yosemite
+ ### Tweak progress bar - artistic I am not - so pick your own colors !!!
+ ### Only apply to Yosemite
+ ######################################################
+ import platform
+ if platform.system() == 'Darwin':
+
+ (Version,junk,machine) = platform.mac_ver()
+ logger.info("This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars ")
+ yosemite = (10,10,0)
+ tVersion = tuple(map(int, (Version.split("."))))
+
+ if tVersion >= yosemite:
+ logger.info( "Found Yosemite:")
+
+ tcss = """
+ QProgressBar {
+ border: 2px solid grey;
+ border-radius: 5px;
+ text-align: center;
+ }
+ QProgressBar::chunk {
+ background-color: #05B8CC;
+ }
+ """
+ self.setStyleSheet(tcss)
+
+ else:
+ logger.info( "Pre-Yosemite")
+
+ ######################################################
+
+ self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
+ rw_cache=sys.path[1] + "/cache")
+
+ cflib.crtp.init_drivers(enable_debug_driver=Config()
+ .get("enable_debug_driver"))
+
+ zmq_params = ZMQParamAccess(self.cf)
+ zmq_params.start()
+
+ zmq_leds = ZMQLEDDriver(self.cf)
+ zmq_leds.start()
+
+ # Create the connection dialogue
+ self.connectDialogue = ConnectDialogue()
+
+ # Create and start the Input Reader
+ self._statusbar_label = QLabel("No input-device found, insert one to"
+ " fly.")
+ self.statusBar().addWidget(self._statusbar_label)
+
+ self.joystickReader = JoystickReader()
+ self._active_device = ""
+ #self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
+
+ self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
+
+ # TODO: Need to reload configs
+ #ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
+
+ # Connections for the Connect Dialogue
+ self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
+
+ self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit)
+ self.connectionFailedSignal.connect(self._connection_failed)
+
+
+ self._input_device_error_signal.connect(self._display_input_device_error)
+ self.joystickReader.device_error.add_callback(
+ self._input_device_error_signal.emit)
+ self._input_discovery_signal.connect(self.device_discovery)
+ self.joystickReader.device_discovery.add_callback(
+ self._input_discovery_signal.emit)
+
+ # Connect UI signals
+ self.menuItemConnect.triggered.connect(self._connect)
+ self.logConfigAction.triggered.connect(self._show_connect_dialog)
+ self.connectButton.clicked.connect(self._connect)
+ self.quickConnectButton.clicked.connect(self._quick_connect)
+ self.menuItemQuickConnect.triggered.connect(self._quick_connect)
+ self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog)
+ self.menuItemExit.triggered.connect(self.closeAppRequest)
+ self.batteryUpdatedSignal.connect(self._update_vbatt)
+ self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
+ self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder)
+
+ self._auto_reconnect_enabled = Config().get("auto_reconnect")
+ self.autoReconnectCheckBox.toggled.connect(
+ self._auto_reconnect_changed)
+ self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
+
+ self.joystickReader.input_updated.add_callback(
+ self.cf.commander.send_setpoint)
+
+ # Connection callbacks and signal wrappers for UI protection
+ self.cf.connected.add_callback(self.connectionDoneSignal.emit)
+ self.connectionDoneSignal.connect(self._connected)
+ self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
+ self.disconnectedSignal.connect(
+ lambda linkURI: self._update_ui_state(UIState.DISCONNECTED,
+ linkURI))
+ self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
+ self.connectionLostSignal.connect(self._connection_lost)
+ self.cf.connection_requested.add_callback(
+ self.connectionInitiatedSignal.emit)
+ self.connectionInitiatedSignal.connect(
+ lambda linkURI: self._update_ui_state(UIState.CONNECTING,
+ linkURI))
+ self._log_error_signal.connect(self._logging_error)
+
+ # Connect link quality feedback
+ self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
+ self.linkQualitySignal.connect(
+ lambda percentage: self.linkQualityBar.setValue(percentage))
+
+ # Set UI state in disconnected buy default
+ self._update_ui_state(UIState.DISCONNECTED)
+
+ # Parse the log configuration files
+ self.logConfigReader = LogConfigReader(self.cf)
+
+ self._current_input_config = None
+ self._active_config = None
+ self._active_config = None
+
+ self.inputConfig = None
+
+ # Add things to helper so tabs can access it
+ cfclient.ui.pluginhelper.cf = self.cf
+ cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
+ cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
+
+ self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
+ self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
+ self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
+ self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper)
+ self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
+ self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
+ self.menuItemAbout.triggered.connect(self._about_dialog.show)
+ self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
+ self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
+
+ # Loading toolboxes (A bit of magic for a lot of automatic)
+ self.toolboxes = []
+ self.toolboxesMenuItem.setMenu(QtGui.QMenu())
+ for t_class in cfclient.ui.toolboxes.toolboxes:
+ toolbox = t_class(cfclient.ui.pluginhelper)
+ dockToolbox = MyDockWidget(toolbox.getName())
+ dockToolbox.setWidget(toolbox)
+ self.toolboxes += [dockToolbox, ]
+
+ # Add menu item for the toolbox
+ item = QtGui.QAction(toolbox.getName(), self)
+ item.setCheckable(True)
+ item.triggered.connect(self.toggleToolbox)
+ self.toolboxesMenuItem.menu().addAction(item)
+
+ dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
+
+ # Setup some introspection
+ item.dockToolbox = dockToolbox
+ item.menuItem = item
+ dockToolbox.dockToolbox = dockToolbox
+ dockToolbox.menuItem = item
+
+ # Load and connect tabs
+ self.tabsMenuItem.setMenu(QtGui.QMenu())
+ tabItems = {}
+ self.loadedTabs = []
+ for tabClass in cfclient.ui.tabs.available:
+ tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
+ item = QtGui.QAction(tab.getMenuName(), self)
+ item.setCheckable(True)
+ item.toggled.connect(tab.toggleVisibility)
+ self.tabsMenuItem.menu().addAction(item)
+ tabItems[tab.getTabName()] = item
+ self.loadedTabs.append(tab)
+ if not tab.enabled:
+ item.setEnabled(False)
+
+ # First instantiate all tabs and then open them in the correct order
+ try:
+ for tName in Config().get("open_tabs").split(","):
+ t = tabItems[tName]
+ if (t != None and t.isEnabled()):
+ # Toggle though menu so it's also marked as open there
+ t.toggle()
+ except Exception as e:
+ logger.warning("Exception while opening tabs [{}]".format(e))
+
+ # References to all the device sub-menus in the "Input device" menu
+ self._all_role_menus = ()
+ # Used to filter what new devices to add default mapping to
+ self._available_devices = ()
+ # Keep track of mux nodes so we can enable according to how many
+ # devices we have
+ self._all_mux_nodes = ()
+
+ # Check which Input muxes are available
+ self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
+ for m in self.joystickReader.available_mux():
+ node = QAction(m.name,
+ self._menu_inputdevice,
+ checkable=True,
+ enabled=False)
+ node.toggled.connect(self._mux_selected)
+ self._mux_group.addAction(node)
+ self._menu_inputdevice.addAction(node)
+ self._all_mux_nodes += (node, )
+ mux_subnodes = ()
+ for name in m.supported_roles():
+ sub_node = QMenu(" {}".format(name),
+ self._menu_inputdevice,
+ enabled=False)
+ self._menu_inputdevice.addMenu(sub_node)
+ mux_subnodes += (sub_node, )
+ self._all_role_menus += ({"muxmenu": node,
+ "rolemenu": sub_node}, )
+ node.setData((m, mux_subnodes))
+
+ self._mapping_support = True
+
+ def _update_ui_state(self, newState, linkURI=""):
+ self.uiState = newState
+ if newState == UIState.DISCONNECTED:
+ self.setWindowTitle("Not connected")
+ self.menuItemConnect.setText("Connect to Crazyflie")
+ self.connectButton.setText("Connect")
+ self.menuItemQuickConnect.setEnabled(True)
+ self.batteryBar.setValue(3000)
+ self._menu_cf2_config.setEnabled(False)
+ self._menu_cf1_config.setEnabled(True)
+ self.linkQualityBar.setValue(0)
+ self.menuItemBootloader.setEnabled(True)
+ self.logConfigAction.setEnabled(False)
+ if len(Config().get("link_uri")) > 0:
+ self.quickConnectButton.setEnabled(True)
+ if newState == UIState.CONNECTED:
+ s = "Connected on %s" % linkURI
+ self.setWindowTitle(s)
+ self.menuItemConnect.setText("Disconnect")
+ self.connectButton.setText("Disconnect")
+ self.logConfigAction.setEnabled(True)
+ # Find out if there's an I2C EEPROM, otherwise don't show the
+ # dialog.
+ if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
+ self._menu_cf2_config.setEnabled(True)
+ self._menu_cf1_config.setEnabled(False)
+ if newState == UIState.CONNECTING:
+ s = "Connecting to {} ...".format(linkURI)
+ self.setWindowTitle(s)
+ self.menuItemConnect.setText("Cancel")
+ self.connectButton.setText("Cancel")
+ self.quickConnectButton.setEnabled(False)
+ self.menuItemBootloader.setEnabled(False)
+ self.menuItemQuickConnect.setEnabled(False)
+
+ @pyqtSlot(bool)
+ def toggleToolbox(self, display):
+ menuItem = self.sender().menuItem
+ dockToolbox = self.sender().dockToolbox
+
+ if display and not dockToolbox.isVisible():
+ dockToolbox.widget().enable()
+ self.addDockWidget(dockToolbox.widget().preferedDockArea(),
+ dockToolbox)
+ dockToolbox.show()
+ elif not display:
+ dockToolbox.widget().disable()
+ self.removeDockWidget(dockToolbox)
+ dockToolbox.hide()
+ menuItem.setChecked(False)
+
+ def _rescan_devices(self):
+ self._statusbar_label.setText("No inputdevice connected!")
+ self._menu_devices.clear()
+ self._active_device = ""
+ self.joystickReader.stop_input()
+
+ #for c in self._menu_mappings.actions():
+ # c.setEnabled(False)
+ #devs = self.joystickReader.available_devices()
+ #if (len(devs) > 0):
+ # self.device_discovery(devs)
+
+ def _show_input_device_config_dialog(self):
+ self.inputConfig = InputConfigDialogue(self.joystickReader)
+ self.inputConfig.show()
+
+ def _auto_reconnect_changed(self, checked):
+ self._auto_reconnect_enabled = checked
+ Config().set("auto_reconnect", checked)
+ logger.info("Auto reconnect enabled: {}".format(checked))
+
+ def _show_connect_dialog(self):
+ self.logConfigDialogue.show()
+
+ def _update_vbatt(self, timestamp, data, logconf):
+ self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
+
+ def _connected(self, linkURI):
+ self._update_ui_state(UIState.CONNECTED, linkURI)
+
+ Config().set("link_uri", str(linkURI))
+
+ lg = LogConfig("Battery", 1000)
+ lg.add_variable("pm.vbat", "float")
+ try:
+ self.cf.log.add_config(lg)
+ lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
+ lg.error_cb.add_callback(self._log_error_signal.emit)
+ lg.start()
+ except KeyError as e:
+ logger.warning(str(e))
+
+ mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0]
+ mem.write_data(self._led_write_done)
+
+ #self._led_write_test = 0
+
+ #mem.leds[self._led_write_test] = [10, 20, 30]
+ #mem.write_data(self._led_write_done)
+
+ def _led_write_done(self, mem, addr):
+ logger.info("LED write done callback")
+ #self._led_write_test += 1
+ #mem.leds[self._led_write_test] = [10, 20, 30]
+ #mem.write_data(self._led_write_done)
+
+ def _logging_error(self, log_conf, msg):
+ QMessageBox.about(self, "Log error", "Error when starting log config"
+ " [{}]: {}".format(log_conf.name, msg))
+
+ def _connection_lost(self, linkURI, msg):
+ if not self._auto_reconnect_enabled:
+ if self.isActiveWindow():
+ warningCaption = "Communication failure"
+ error = "Connection lost to {}: {}".format(linkURI, msg)
+ QMessageBox.critical(self, warningCaption, error)
+ self._update_ui_state(UIState.DISCONNECTED, linkURI)
+ else:
+ self._quick_connect()
+
+ def _connection_failed(self, linkURI, error):
+ if not self._auto_reconnect_enabled:
+ msg = "Failed to connect on {}: {}".format(linkURI, error)
+ warningCaption = "Communication failure"
+ QMessageBox.critical(self, warningCaption, msg)
+ self._update_ui_state(UIState.DISCONNECTED, linkURI)
+ else:
+ self._quick_connect()
+
+ def closeEvent(self, event):
+ self.hide()
+ self.cf.close_link()
+ Config().save_file()
+
+ def _connect(self):
+ if self.uiState == UIState.CONNECTED:
+ self.cf.close_link()
+ elif self.uiState == UIState.CONNECTING:
+ self.cf.close_link()
+ self._update_ui_state(UIState.DISCONNECTED)
+ else:
+ self.connectDialogue.show()
+
+ def _display_input_device_error(self, error):
+ self.cf.close_link()
+ QMessageBox.critical(self, "Input device error", error)
+
+ def _mux_selected(self, checked):
+ """Called when a new mux is selected. The menu item contains a
+ reference to the raw mux object as well as to the associated device
+ sub-nodes"""
+ if not checked:
+ (mux, sub_nodes) = self.sender().data().toPyObject()
+ for s in sub_nodes:
+ s.setEnabled(False)
+ else:
+ (mux, sub_nodes) = self.sender().data().toPyObject()
+ for s in sub_nodes:
+ s.setEnabled(True)
+ self.joystickReader.set_mux(mux=mux)
+
+ # Go though the tree and select devices/mapping that was
+ # selected before it was disabled.
+ for role_node in sub_nodes:
+ for dev_node in role_node.children():
+ if type(dev_node) is QAction and dev_node.isChecked():
+ dev_node.toggled.emit(True)
+
+ self._update_input_device_footer()
+
+ def _get_dev_status(self, device):
+ msg = "{}".format(device.name)
+ if device.supports_mapping:
+ map_name = "N/A"
+ if device.input_map:
+ map_name = device.input_map_name
+ msg += " ({})".format(map_name)
+ return msg
+
+ def _update_input_device_footer(self):
+ """Update the footer in the bottom of the UI with status for the
+ input device and its mapping"""
+
+ msg = ""
+
+ if len(self.joystickReader.available_devices()) > 0:
+ mux = self.joystickReader._selected_mux
+ msg = "Using {} mux with ".format(mux.name)
+ for key in mux._devs.keys()[:-1]:
+ if mux._devs[key]:
+ msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
+ else:
+ msg += "N/A, "
+ # Last item
+ key = mux._devs.keys()[-1]
+ if mux._devs[key]:
+ msg += "{}".format(self._get_dev_status(mux._devs[key]))
+ else:
+ msg += "N/A"
+ else:
+ msg = "No input device found"
+ self._statusbar_label.setText(msg)
+
+ def _inputdevice_selected(self, checked):
+ """Called when a new input device has been selected from the menu. The
+ data in the menu object is the associated map menu (directly under the
+ item in the menu) and the raw device"""
+ (map_menu, device, mux_menu) = self.sender().data().toPyObject()
+ if not checked:
+ if map_menu:
+ map_menu.setEnabled(False)
+ # Do not close the device, since we don't know exactly
+ # how many devices the mux can have open. When selecting a
+ # new mux the old one will take care of this.
+ else:
+ if map_menu:
+ map_menu.setEnabled(True)
+
+ (mux, sub_nodes) = mux_menu.data().toPyObject()
+ for role_node in sub_nodes:
+ for dev_node in role_node.children():
+ if type(dev_node) is QAction and dev_node.isChecked():
+ if device.id == dev_node.data().toPyObject()[1].id \
+ and dev_node is not self.sender():
+ dev_node.setChecked(False)
+
+ role_in_mux = str(self.sender().parent().title()).strip()
+ logger.info("Role of {} is {}".format(device.name,
+ role_in_mux))
+
+ Config().set("input_device", str(device.name))
+
+ self._mapping_support = self.joystickReader.start_input(device.name,
+ role_in_mux)
+ self._update_input_device_footer()
+
+ def _inputconfig_selected(self, checked):
+ """Called when a new configuration has been selected from the menu. The
+ data in the menu object is a referance to the device QAction in parent
+ menu. This contains a referance to the raw device."""
+ if not checked:
+ return
+
+ selected_mapping = str(self.sender().text())
+ device = self.sender().data().toPyObject().data().toPyObject()[1]
+ self.joystickReader.set_input_map(device.name, selected_mapping)
+ self._update_input_device_footer()
+
+ def device_discovery(self, devs):
+ """Called when new devices have been added"""
+ for menu in self._all_role_menus:
+ role_menu = menu["rolemenu"]
+ mux_menu = menu["muxmenu"]
+ dev_group = QActionGroup(role_menu, exclusive=True)
+ for d in devs:
+ dev_node = QAction(d.name, role_menu, checkable=True,
+ enabled=True)
+ role_menu.addAction(dev_node)
+ dev_group.addAction(dev_node)
+ dev_node.toggled.connect(self._inputdevice_selected)
+
+ map_node = None
+ if d.supports_mapping:
+ map_node = QMenu(" Input map", role_menu, enabled=False)
+ map_group = QActionGroup(role_menu, exclusive=True)
+ # Connect device node to map node for easy
+ # enabling/disabling when selection changes and device
+ # to easily enable it
+ dev_node.setData((map_node, d))
+ for c in ConfigManager().get_list_of_configs():
+ node = QAction(c, map_node, checkable=True,
+ enabled=True)
+ node.toggled.connect(self._inputconfig_selected)
+ map_node.addAction(node)
+ # Connect all the map nodes back to the device
+ # action node where we can access the raw device
+ node.setData(dev_node)
+ map_group.addAction(node)
+ # If this device hasn't been found before, then
+ # select the default mapping for it.
+ if d not in self._available_devices:
+ last_map = Config().get("device_config_mapping")
+ if last_map.has_key(d.name) and last_map[d.name] == c:
+ node.setChecked(True)
+ role_menu.addMenu(map_node)
+ dev_node.setData((map_node, d, mux_menu))
+
+ # Update the list of what devices we found
+ # to avoid selecting default mapping for all devices when
+ # a new one is inserted
+ self._available_devices = ()
+ for d in devs:
+ self._available_devices += (d, )
+
+ # Only enable MUX nodes if we have enough devies to cover
+ # the roles
+ for mux_node in self._all_mux_nodes:
+ (mux, sub_nodes) = mux_node.data().toPyObject()
+ if len(mux.supported_roles()) <= len(self._available_devices):
+ mux_node.setEnabled(True)
+
+ # TODO: Currently only supports selecting default mux
+ if self._all_mux_nodes[0].isEnabled():
+ self._all_mux_nodes[0].setChecked(True)
+
+ # If the previous length of the available devies was 0, then select
+ # the default on. If that's not available then select the first
+ # on in the list.
+ # TODO: This will only work for the "Normal" mux so this will be
+ # selected by default
+ if Config().get("input_device") in [d.name for d in devs]:
+ for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
+ if dev_menu.text() == Config().get("input_device"):
+ dev_menu.setChecked(True)
+ else:
+ # Select the first device in the first mux (will always be "Normal"
+ # mux)
+ self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
+ logger.info("Select first device")
+
+ self._update_input_device_footer()
+
+ def _quick_connect(self):
+ try:
+ self.cf.open_link(Config().get("link_uri"))
+ except KeyError:
+ self.cf.open_link("")
+
+ def _open_config_folder(self):
+ QDesktopServices.openUrl(QUrl("file:///" +
+ QDir.toNativeSeparators(sys.path[1])))
+
+ def closeAppRequest(self):
+ self.close()
+ sys.exit(0)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..354b7bcf1879459e213634c9b237de32a70a699e
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.ui
new file mode 100755
index 0000000000000000000000000000000000000000..25cd15f668eaac66714a188b73bc3983432c2629
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/main.ui
@@ -0,0 +1,375 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>872</width>
+ <height>641</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Crazyflie Nano Quadcopter Control</string>
+ </property>
+ <property name="statusTip">
+ <string/>
+ </property>
+ <widget class="QWidget" name="centralwidget">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="quickConnectButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="toolTip">
+ <string><html><head/><body><p>Quickconnect using the last connection parameters</p></body></html></string>
+ </property>
+ <property name="text">
+ <string>Quick Connect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <spacer name="horizontalSpacer">
+ <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 row="0" column="5">
+ <widget class="QProgressBar" name="linkQualityBar">
+ <property name="minimumSize">
+ <size>
+ <width>150</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>100</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="textVisible">
+ <bool>true</bool>
+ </property>
+ <property name="format">
+ <string>Link quality %p%</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="connectButton">
+ <property name="toolTip">
+ <string><html><head/><body><p>Connect to a Crazyflie</p></body></html></string>
+ </property>
+ <property name="text">
+ <string>Connect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QProgressBar" name="batteryBar">
+ <property name="minimumSize">
+ <size>
+ <width>150</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>150</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="minimum">
+ <number>3000</number>
+ </property>
+ <property name="maximum">
+ <number>4200</number>
+ </property>
+ <property name="value">
+ <number>3000</number>
+ </property>
+ <property name="textVisible">
+ <bool>true</bool>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="textDirection">
+ <enum>QProgressBar::TopToBottom</enum>
+ </property>
+ <property name="format">
+ <string>Battery %v mV</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QCheckBox" name="autoReconnectCheckBox">
+ <property name="mouseTracking">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Auto Reconnect</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QTabWidget" name="tabs">
+ <property name="currentIndex">
+ <number>-1</number>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QMenuBar" name="menubar">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>872</width>
+ <height>25</height>
+ </rect>
+ </property>
+ <widget class="QMenu" name="menuFile">
+ <property name="title">
+ <string>File</string>
+ </property>
+ <addaction name="separator"/>
+ <addaction name="menuItemExit"/>
+ </widget>
+ <widget class="QMenu" name="menuView">
+ <property name="title">
+ <string>View</string>
+ </property>
+ <addaction name="tabsMenuItem"/>
+ <addaction name="toolboxesMenuItem"/>
+ </widget>
+ <widget class="QMenu" name="menuHelp">
+ <property name="title">
+ <string>Help</string>
+ </property>
+ <addaction name="menuItemAbout"/>
+ </widget>
+ <widget class="QMenu" name="menuSettings">
+ <property name="title">
+ <string>Settings</string>
+ </property>
+ <addaction name="logConfigAction"/>
+ <addaction name="separator"/>
+ <addaction name="_menuItem_openconfigfolder"/>
+ </widget>
+ <widget class="QMenu" name="menuCrazyflie">
+ <property name="title">
+ <string>Crazyflie</string>
+ </property>
+ <addaction name="menuItemConnect"/>
+ <addaction name="menuItemQuickConnect"/>
+ <addaction name="menuItemBootloader"/>
+ <addaction name="_menu_cf2_config"/>
+ <addaction name="_menu_cf1_config"/>
+ </widget>
+ <widget class="QMenu" name="_menu_inputdevice">
+ <property name="title">
+ <string>Input device</string>
+ </property>
+ <addaction name="menuItemConfInputDevice"/>
+ <addaction name="_menuitem_rescandevices"/>
+ <addaction name="separator"/>
+ </widget>
+ <addaction name="menuFile"/>
+ <addaction name="menuCrazyflie"/>
+ <addaction name="_menu_inputdevice"/>
+ <addaction name="menuSettings"/>
+ <addaction name="menuView"/>
+ <addaction name="menuHelp"/>
+ </widget>
+ <widget class="QStatusBar" name="statusbar"/>
+ <action name="menuItemConnect">
+ <property name="text">
+ <string>Connect</string>
+ </property>
+ </action>
+ <action name="menuItemExit">
+ <property name="text">
+ <string>Exit</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="menuItemRegulation">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Regulation</string>
+ </property>
+ </action>
+ <action name="menuItemPlot">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Plotting</string>
+ </property>
+ </action>
+ <action name="menuItemFlightdata">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Flight data</string>
+ </property>
+ </action>
+ <action name="menuItemService">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Service</string>
+ </property>
+ </action>
+ <action name="menuItemAbout">
+ <property name="text">
+ <string>About</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="menuItemQuickConnect">
+ <property name="text">
+ <string>Quick connect</string>
+ </property>
+ </action>
+ <action name="menuItemConfInputDevice">
+ <property name="text">
+ <string>Configure device mapping</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="actionSelect_input_device">
+ <property name="text">
+ <string>Select input device</string>
+ </property>
+ </action>
+ <action name="menuItemConsole">
+ <property name="text">
+ <string>Console</string>
+ </property>
+ </action>
+ <action name="toolboxesMenuItem">
+ <property name="text">
+ <string>Toolboxes</string>
+ </property>
+ </action>
+ <action name="tabsMenuItem">
+ <property name="text">
+ <string>Tabs</string>
+ </property>
+ </action>
+ <action name="logConfigAction">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Logging configurations</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="actionOptions">
+ <property name="text">
+ <string>Options</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="menuItemBootloader">
+ <property name="text">
+ <string>Bootloader</string>
+ </property>
+ </action>
+ <action name="actionAsdfasdf">
+ <property name="text">
+ <string>asdfasdf</string>
+ </property>
+ </action>
+ <action name="actionSdfasdf">
+ <property name="text">
+ <string>sdfasdf</string>
+ </property>
+ </action>
+ <action name="_menuitem_rescandevices">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Rescan devices</string>
+ </property>
+ </action>
+ <action name="_menuItem_openconfigfolder">
+ <property name="text">
+ <string>Open config folder</string>
+ </property>
+ </action>
+ <action name="_menu_cf2_config">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Configure 2.0</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ <action name="_menu_cf_config_2">
+ <property name="text">
+ <string>Configure 1.0</string>
+ </property>
+ </action>
+ <action name="_menu_cf1_config">
+ <property name="text">
+ <string>Configure 1.0</string>
+ </property>
+ <property name="menuRole">
+ <enum>QAction::NoRole</enum>
+ </property>
+ </action>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.py
new file mode 100755
index 0000000000000000000000000000000000000000..36cdde036588dfc6c9bb540ef5af7fcafdfea969
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Used for passing objects to tabs and toolboxes.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PluginHelper']
+
+
+class PluginHelper():
+ """Used for passing objects to tabs and toolboxes"""
+ def __init__(self):
+ self.cf = None
+ self.menu = None
+ self.logConfigReader = None
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..06a84b6b0cc14e0ab6ae757a2f890ea9d224a53a
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/pluginhelper.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.py
new file mode 100755
index 0000000000000000000000000000000000000000..3913e34184c7f5ffa05552aa1092a4b80438772f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.py
@@ -0,0 +1,92 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Superclass for all tabs that implements common functions.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Tab']
+
+import logging
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cfclient.utils.config import Config
+
+
+class Tab(QtGui.QWidget):
+ """Superclass for all tabs that implements common functions."""
+
+ def __init__(self):
+ super(Tab, self).__init__()
+ self.tabName = "N/A"
+ self.menuName = "N/A"
+ self.enabled = True
+
+ @pyqtSlot(bool)
+ def toggleVisibility(self, checked):
+ """Show or hide the tab."""
+ if checked:
+ self.tabWidget.addTab(self, self.getTabName())
+ s = ""
+ try:
+ s = Config().get("open_tabs")
+ if (len(s) > 0):
+ s += ","
+ except Exception as e:
+ logger.warning("Exception while adding tab to config and "
+ "reading tab config")
+ # Check this since tabs in config are opened when app is started
+ if (self.tabName not in s):
+ s += "%s" % self.tabName
+ Config().set("open_tabs", str(s))
+
+ if not checked:
+ self.tabWidget.removeTab(self.tabWidget.indexOf(self))
+ try:
+ parts = Config().get("open_tabs").split(",")
+ except Exception as e:
+ logger.warning("Exception while removing tab from config and "
+ "reading tab config")
+ parts = []
+ s = ""
+ for p in parts:
+ if (self.tabName != p):
+ s += "%s," % p
+ s = s[0:len(s) - 1] # Remove last comma
+ Config().set("open_tabs", str(s))
+
+ def getMenuName(self):
+ """Return the name of the tab that will be shown in the menu"""
+ return self.menuName
+
+ def getTabName(self):
+ """Return the name of the tab that will be shown in the tab"""
+ return self.tabName
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..a18916cffa5e79aa6b00b247f4326c0ec69183ce
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..a90e1bed887c55cba1de2d325934916546266f32
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The console tab is used as a console for printouts from the Crazyflie.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ConsoleTab']
+
+import time
+import sys
+
+import logging
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import pyqtSlot, pyqtSignal
+
+from cfclient.ui.tab import Tab
+
+console_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/consoleTab.ui")[0]
+
+
+class ConsoleTab(Tab, console_tab_class):
+ """Console tab for showing printouts from Crazyflie"""
+ update = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(ConsoleTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Console"
+ self.menuName = "Console"
+
+ self.tabWidget = tabWidget
+ self.helper = helper
+
+ self.update.connect(self.printText)
+
+ self.helper.cf.console.receivedChar.add_callback(self.update.emit)
+
+ def printText(self, text):
+ # Make sure we get printouts from the Crazyflie into the log (such as
+ # build version and test ok/fail)
+ logger.debug("[%s]", text)
+ self.console.insertPlainText(text)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..979bf106dc4e463400fc56259c980787123275fa
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ConsoleTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..e34870a950112a2a2b9f3a855f0f2c228592c39a
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+
+"""
+An example template for a tab in the Crazyflie Client. It comes pre-configured
+with the necessary QT Signals to wrap Crazyflie API callbacks and also
+connects the connected/disconnected callbacks.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ExampleTab']
+
+import logging
+import sys
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import pyqtSlot, pyqtSignal, QThread, Qt
+from PyQt4.QtGui import QMessageBox
+
+from cfclient.ui.tab import Tab
+
+from cflib.crazyflie.log import LogConfig, Log
+from cflib.crazyflie.param import Param
+
+example_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/exampleTab.ui")[0]
+
+class ExampleTab(Tab, example_tab_class):
+ """Tab for plotting logging data"""
+
+ _connected_signal = pyqtSignal(str)
+ _disconnected_signal = pyqtSignal(str)
+ _log_data_signal = pyqtSignal(int, object, object)
+ _log_error_signal = pyqtSignal(object, str)
+ _param_updated_signal = pyqtSignal(str, str)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(ExampleTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Example"
+ self.menuName = "Example Tab"
+ self.tabWidget = tabWidget
+
+ self._helper = helper
+
+ # Always wrap callbacks from Crazyflie API though QT Signal/Slots
+ # to avoid manipulating the UI when rendering it
+ self._connected_signal.connect(self._connected)
+ self._disconnected_signal.connect(self._disconnected)
+ self._log_data_signal.connect(self._log_data_received)
+ self._param_updated_signal.connect(self._param_updated)
+
+ # Connect the Crazyflie API callbacks to the signals
+ self._helper.cf.connected.add_callback(
+ self._connected_signal.emit)
+
+ self._helper.cf.disconnected.add_callback(
+ self._disconnected_signal.emit)
+
+ def _connected(self, link_uri):
+ """Callback when the Crazyflie has been connected"""
+
+ logger.debug("Crazyflie connected to {}".format(link_uri))
+
+ def _disconnected(self, link_uri):
+ """Callback for when the Crazyflie has been disconnected"""
+
+ logger.debug("Crazyflie disconnected from {}".format(link_uri))
+
+ def _param_updated(self, name, value):
+ """Callback when the registered parameter get's updated"""
+
+ logger.debug("Updated {0} to {1}".format(name, value))
+
+ def _log_data_received(self, timestamp, data, log_conf):
+ """Callback when the log layer receives new data"""
+
+ logger.debug("{0}:{1}:{2}".format(timestamp, log_conf.name, data))
+
+ def _logging_error(self, log_conf, msg):
+ """Callback from the log layer when an error occurs"""
+
+ QMessageBox.about(self, "Example error",
+ "Error when using log config"
+ " [{0}]: {1}".format(log_conf.name, msg))
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..8ef24d306d402aa0303d10941dfbb52747162bff
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ExampleTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..8995debbd79a99496d5f67aaed5c0d0ff6f4a019
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.py
@@ -0,0 +1,556 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+The flight control tab shows telemetry data and flight settings.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['FlightTab']
+
+import sys
+
+import logging
+logger = logging.getLogger(__name__)
+
+from time import time
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QVariant
+from PyQt4.QtGui import QMessageBox
+
+from cflib.crazyflie import Crazyflie
+
+from cfclient.ui.widgets.ai import AttitudeIndicator
+
+from cfclient.utils.config import Config
+from cflib.crazyflie.log import Log, LogVariable, LogConfig
+
+from cfclient.ui.tab import Tab
+
+from cflib.crazyflie.mem import MemoryElement
+
+flight_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/flightTab.ui")[0]
+
+MAX_THRUST = 65365.0
+
+
+class FlightTab(Tab, flight_tab_class):
+
+ uiSetupReadySignal = pyqtSignal()
+
+ _motor_data_signal = pyqtSignal(int, object, object)
+ _imu_data_signal = pyqtSignal(int, object, object)
+ _althold_data_signal = pyqtSignal(int, object, object)
+ _baro_data_signal = pyqtSignal(int, object, object)
+
+ _input_updated_signal = pyqtSignal(float, float, float, float)
+ _rp_trim_updated_signal = pyqtSignal(float, float)
+ _emergency_stop_updated_signal = pyqtSignal(bool)
+
+ _log_error_signal = pyqtSignal(object, str)
+
+ #UI_DATA_UPDATE_FPS = 10
+
+ connectionFinishedSignal = pyqtSignal(str)
+ disconnectedSignal = pyqtSignal(str)
+
+ _limiting_updated = pyqtSignal(bool, bool, bool)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(FlightTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Flight Control"
+ self.menuName = "Flight Control"
+
+ self.tabWidget = tabWidget
+ self.helper = helper
+
+ self.disconnectedSignal.connect(self.disconnected)
+ self.connectionFinishedSignal.connect(self.connected)
+ # Incomming signals
+ self.helper.cf.connected.add_callback(
+ self.connectionFinishedSignal.emit)
+ self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
+
+ self._input_updated_signal.connect(self.updateInputControl)
+ self.helper.inputDeviceReader.input_updated.add_callback(
+ self._input_updated_signal.emit)
+ self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
+ self.helper.inputDeviceReader.rp_trim_updated.add_callback(
+ self._rp_trim_updated_signal.emit)
+ self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
+ self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
+ self._emergency_stop_updated_signal.emit)
+
+ self.helper.inputDeviceReader.althold_updated.add_callback(
+ lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
+
+ self._imu_data_signal.connect(self._imu_data_received)
+ self._baro_data_signal.connect(self._baro_data_received)
+ self._althold_data_signal.connect(self._althold_data_received)
+ self._motor_data_signal.connect(self._motor_data_received)
+
+ self._log_error_signal.connect(self._logging_error)
+
+ # Connect UI signals that are in this tab
+ self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
+ self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
+ self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
+ self.thrustLoweringSlewRateLimit.valueChanged.connect(
+ self.thrustLoweringSlewRateLimitChanged)
+ self.slewEnableLimit.valueChanged.connect(
+ self.thrustLoweringSlewRateLimitChanged)
+ self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
+ self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
+ self.maxAngle.valueChanged.connect(self.maxAngleChanged)
+ self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
+ self.uiSetupReadySignal.connect(self.uiSetupReady)
+ self.clientXModeCheckbox.toggled.connect(self.changeXmode)
+ self.isInCrazyFlightmode = False
+ self.uiSetupReady()
+
+ self.clientXModeCheckbox.setChecked(Config().get("client_side_xmode"))
+
+ self.crazyflieXModeCheckbox.clicked.connect(
+ lambda enabled:
+ self.helper.cf.param.set_value("flightmode.x",
+ str(enabled)))
+ self.helper.cf.param.add_update_callback(
+ group="flightmode", name="xmode",
+ cb=( lambda name, checked:
+ self.crazyflieXModeCheckbox.setChecked(eval(checked))))
+
+ self.ratePidRadioButton.clicked.connect(
+ lambda enabled:
+ self.helper.cf.param.set_value("flightmode.ratepid",
+ str(enabled)))
+
+ self.angularPidRadioButton.clicked.connect(
+ lambda enabled:
+ self.helper.cf.param.set_value("flightmode.ratepid",
+ str(not enabled)))
+
+ self._led_ring_headlight.clicked.connect(
+ lambda enabled:
+ self.helper.cf.param.set_value("ring.headlightEnable",
+ str(enabled)))
+
+ self.helper.cf.param.add_update_callback(
+ group="flightmode", name="ratepid",
+ cb=(lambda name, checked:
+ self.ratePidRadioButton.setChecked(eval(checked))))
+
+ self.helper.cf.param.add_update_callback(
+ group="cpu", name="flash",
+ cb=self._set_enable_client_xmode)
+
+ self.helper.cf.param.add_update_callback(
+ group="ring", name="headlightEnable",
+ cb=(lambda name, checked:
+ self._led_ring_headlight.setChecked(eval(checked))))
+
+ self.helper.cf.param.add_update_callback(
+ group="flightmode", name="althold",
+ cb=(lambda name, enabled:
+ self.helper.inputDeviceReader.enable_alt_hold(eval(enabled))))
+
+ self._ledring_nbr_effects = 0
+
+ self.helper.cf.param.add_update_callback(
+ group="ring",
+ name="neffect",
+ cb=(lambda name, value: self._set_neffect(eval(value))))
+
+ self.helper.cf.param.add_update_callback(
+ group="imu_sensors",
+ cb=self._set_available_sensors)
+
+ self.helper.cf.param.all_updated.add_callback(self._ring_populate_dropdown)
+
+ self.logBaro = None
+ self.logAltHold = None
+
+ self.ai = AttitudeIndicator()
+ self.verticalLayout_4.addWidget(self.ai)
+ self.splitter.setSizes([1000,1])
+
+ self.targetCalPitch.setValue(Config().get("trim_pitch"))
+ self.targetCalRoll.setValue(Config().get("trim_roll"))
+
+ self.helper.inputDeviceReader.alt1_updated.add_callback(self.alt1_updated)
+ self.helper.inputDeviceReader.alt2_updated.add_callback(self.alt2_updated)
+ self._tf_state = 0
+ self._ring_effect = 0
+
+ # Connect callbacks for input device limiting of rpöö/pitch/yaw/thust
+ self.helper.inputDeviceReader.limiting_updated.add_callback(
+ self._limiting_updated.emit)
+ self._limiting_updated.connect(self._set_limiting_enabled)
+
+ def _set_enable_client_xmode(self, name, value):
+ if eval(value) <= 128:
+ self.clientXModeCheckbox.setEnabled(True)
+ else:
+ self.clientXModeCheckbox.setEnabled(False)
+ self.clientXModeCheckbox.setChecked(False)
+
+ def _set_limiting_enabled(self, rp_limiting_enabled,
+ yaw_limiting_enabled,
+ thrust_limiting_enabled):
+ self.maxAngle.setEnabled(rp_limiting_enabled)
+ self.targetCalRoll.setEnabled(rp_limiting_enabled)
+ self.targetCalPitch.setEnabled(rp_limiting_enabled)
+ self.maxYawRate.setEnabled(yaw_limiting_enabled)
+ self.maxThrust.setEnabled(thrust_limiting_enabled)
+ self.minThrust.setEnabled(thrust_limiting_enabled)
+ self.slewEnableLimit.setEnabled(thrust_limiting_enabled)
+ self.thrustLoweringSlewRateLimit.setEnabled(thrust_limiting_enabled)
+
+ def _set_neffect(self, n):
+ self._ledring_nbr_effects = n
+
+ def thrustToPercentage(self, thrust):
+ return ((thrust / MAX_THRUST) * 100.0)
+
+ def uiSetupReady(self):
+ flightComboIndex = self.flightModeCombo.findText(
+ Config().get("flightmode"), Qt.MatchFixedString)
+ if (flightComboIndex < 0):
+ self.flightModeCombo.setCurrentIndex(0)
+ self.flightModeCombo.currentIndexChanged.emit(0)
+ else:
+ self.flightModeCombo.setCurrentIndex(flightComboIndex)
+ self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)
+
+ def _logging_error(self, log_conf, msg):
+ QMessageBox.about(self, "Log error", "Error when starting log config"
+ " [%s]: %s" % (log_conf.name, msg))
+
+ def _motor_data_received(self, timestamp, data, logconf):
+ if self.isVisible():
+ self.actualM1.setValue(data["motor.m1"])
+ self.actualM2.setValue(data["motor.m2"])
+ self.actualM3.setValue(data["motor.m3"])
+ self.actualM4.setValue(data["motor.m4"])
+
+ def _baro_data_received(self, timestamp, data, logconf):
+ if self.isVisible():
+ self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
+ self.ai.setBaro(data["baro.aslLong"])
+
+ def _althold_data_received(self, timestamp, data, logconf):
+ if self.isVisible():
+ target = data["altHold.target"]
+ if target>0:
+ if not self.targetASL.isEnabled():
+ self.targetASL.setEnabled(True)
+ self.targetASL.setText(("%.2f" % target))
+ self.ai.setHover(target)
+ elif self.targetASL.isEnabled():
+ self.targetASL.setEnabled(False)
+ self.targetASL.setText("Not set")
+ self.ai.setHover(0)
+
+ def _imu_data_received(self, timestamp, data, logconf):
+ if self.isVisible():
+ self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
+ self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
+ self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
+ self.actualThrust.setText("%.2f%%" %
+ self.thrustToPercentage(
+ data["stabilizer.thrust"]))
+
+ self.ai.setRollPitch(-data["stabilizer.roll"],
+ data["stabilizer.pitch"])
+
+ def connected(self, linkURI):
+ # IMU & THRUST
+ lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
+ lg.add_variable("stabilizer.roll", "float")
+ lg.add_variable("stabilizer.pitch", "float")
+ lg.add_variable("stabilizer.yaw", "float")
+ lg.add_variable("stabilizer.thrust", "uint16_t")
+
+ try:
+ self.helper.cf.log.add_config(lg)
+ lg.data_received_cb.add_callback(self._imu_data_signal.emit)
+ lg.error_cb.add_callback(self._log_error_signal.emit)
+ lg.start()
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError as e:
+ logger.warning(str(e))
+
+ # MOTOR
+ lg = LogConfig("Motors", Config().get("ui_update_period"))
+ lg.add_variable("motor.m1")
+ lg.add_variable("motor.m2")
+ lg.add_variable("motor.m3")
+ lg.add_variable("motor.m4")
+
+ try:
+ self.helper.cf.log.add_config(lg)
+ lg.data_received_cb.add_callback(self._motor_data_signal.emit)
+ lg.error_cb.add_callback(self._log_error_signal.emit)
+ lg.start()
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError as e:
+ logger.warning(str(e))
+
+ if self.helper.cf.mem.ow_search(vid=0xBC, pid=0x01):
+ self._led_ring_effect.setEnabled(True)
+ self._led_ring_headlight.setEnabled(True)
+
+ def _set_available_sensors(self, name, available):
+ logger.info("[%s]: %s", name, available)
+ available = eval(available)
+ if ("HMC5883L" in name):
+ if (not available):
+ self.actualASL.setText("N/A")
+ self.actualASL.setEnabled(False)
+ else:
+ self.actualASL.setEnabled(True)
+ self.helper.inputDeviceReader.set_alt_hold_available(available)
+ if (not self.logBaro and not self.logAltHold):
+ # The sensor is available, set up the logging
+ self.logBaro = LogConfig("Baro", 200)
+ self.logBaro.add_variable("baro.aslLong", "float")
+
+ try:
+ self.helper.cf.log.add_config(self.logBaro)
+ self.logBaro.data_received_cb.add_callback(
+ self._baro_data_signal.emit)
+ self.logBaro.error_cb.add_callback(
+ self._log_error_signal.emit)
+ self.logBaro.start()
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError as e:
+ logger.warning(str(e))
+ self.logAltHold = LogConfig("AltHold", 200)
+ self.logAltHold.add_variable("altHold.target", "float")
+
+ try:
+ self.helper.cf.log.add_config(self.logAltHold)
+ self.logAltHold.data_received_cb.add_callback(
+ self._althold_data_signal.emit)
+ self.logAltHold.error_cb.add_callback(
+ self._log_error_signal.emit)
+ self.logAltHold.start()
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError:
+ logger.warning(str(e))
+
+ def disconnected(self, linkURI):
+ self.ai.setRollPitch(0, 0)
+ self.actualM1.setValue(0)
+ self.actualM2.setValue(0)
+ self.actualM3.setValue(0)
+ self.actualM4.setValue(0)
+ self.actualRoll.setText("")
+ self.actualPitch.setText("")
+ self.actualYaw.setText("")
+ self.actualThrust.setText("")
+ self.actualASL.setText("")
+ self.targetASL.setText("Not Set")
+ self.targetASL.setEnabled(False)
+ self.actualASL.setEnabled(False)
+ self.clientXModeCheckbox.setEnabled(False)
+ self.logBaro = None
+ self.logAltHold = None
+ self._led_ring_effect.setEnabled(False)
+ self._led_ring_headlight.setEnabled(False)
+
+
+ def minMaxThrustChanged(self):
+ self.helper.inputDeviceReader.min_thrust = self.minThrust.value()
+ self.helper.inputDeviceReader.max_thrust = self.maxThrust.value()
+ if (self.isInCrazyFlightmode == True):
+ Config().set("min_thrust", self.minThrust.value())
+ Config().set("max_thrust", self.maxThrust.value())
+
+ def thrustLoweringSlewRateLimitChanged(self):
+ self.helper.inputDeviceReader.thrust_slew_rate = self.thrustLoweringSlewRateLimit.value()
+ self.helper.inputDeviceReader.thrust_slew_limit = self.slewEnableLimit.value()
+ if (self.isInCrazyFlightmode == True):
+ Config().set("slew_limit", self.slewEnableLimit.value())
+ Config().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
+
+ def maxYawRateChanged(self):
+ logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
+ self.helper.inputDeviceReader.max_yaw_rate = self.maxYawRate.value()
+ if (self.isInCrazyFlightmode == True):
+ Config().set("max_yaw", self.maxYawRate.value())
+
+ def maxAngleChanged(self):
+ logger.debug("MaxAngle changed to %d", self.maxAngle.value())
+ self.helper.inputDeviceReader.max_rp_angle = self.maxAngle.value()
+ if (self.isInCrazyFlightmode == True):
+ Config().set("max_rp", self.maxAngle.value())
+
+ def _trim_pitch_changed(self, value):
+ logger.debug("Pitch trim updated to [%f]" % value)
+ self.helper.inputDeviceReader.trim_pitch = value
+ Config().set("trim_pitch", value)
+
+ def _trim_roll_changed(self, value):
+ logger.debug("Roll trim updated to [%f]" % value)
+ self.helper.inputDeviceReader.trim_roll = value
+ Config().set("trim_roll", value)
+
+ def calUpdateFromInput(self, rollCal, pitchCal):
+ logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
+ rollCal, pitchCal)
+ self.targetCalRoll.setValue(rollCal)
+ self.targetCalPitch.setValue(pitchCal)
+
+ def updateInputControl(self, roll, pitch, yaw, thrust):
+ self.targetRoll.setText(("%0.2f" % roll))
+ self.targetPitch.setText(("%0.2f" % pitch))
+ self.targetYaw.setText(("%0.2f" % yaw))
+ self.targetThrust.setText(("%0.2f %%" %
+ self.thrustToPercentage(thrust)))
+ self.thrustProgress.setValue(thrust)
+
+ def setMotorLabelsEnabled(self, enabled):
+ self.M1label.setEnabled(enabled)
+ self.M2label.setEnabled(enabled)
+ self.M3label.setEnabled(enabled)
+ self.M4label.setEnabled(enabled)
+
+ def emergencyStopStringWithText(self, text):
+ return ("<html><head/><body><p>"
+ "<span style='font-weight:600; color:#7b0005;'>{}</span>"
+ "</p></body></html>".format(text))
+
+ def updateEmergencyStop(self, emergencyStop):
+ if emergencyStop:
+ self.setMotorLabelsEnabled(False)
+ self.emergency_stop_label.setText(
+ self.emergencyStopStringWithText("Kill switch active"))
+ else:
+ self.setMotorLabelsEnabled(True)
+ self.emergency_stop_label.setText("")
+
+ def flightmodeChange(self, item):
+ Config().set("flightmode", str(self.flightModeCombo.itemText(item)))
+ logger.debug("Changed flightmode to %s",
+ self.flightModeCombo.itemText(item))
+ self.isInCrazyFlightmode = False
+ if (item == 0): # Normal
+ self.maxAngle.setValue(Config().get("normal_max_rp"))
+ self.maxThrust.setValue(Config().get("normal_max_thrust"))
+ self.minThrust.setValue(Config().get("normal_min_thrust"))
+ self.slewEnableLimit.setValue(Config().get("normal_slew_limit"))
+ self.thrustLoweringSlewRateLimit.setValue(
+ Config().get("normal_slew_rate"))
+ self.maxYawRate.setValue(Config().get("normal_max_yaw"))
+ if (item == 1): # Advanced
+ self.maxAngle.setValue(Config().get("max_rp"))
+ self.maxThrust.setValue(Config().get("max_thrust"))
+ self.minThrust.setValue(Config().get("min_thrust"))
+ self.slewEnableLimit.setValue(Config().get("slew_limit"))
+ self.thrustLoweringSlewRateLimit.setValue(
+ Config().get("slew_rate"))
+ self.maxYawRate.setValue(Config().get("max_yaw"))
+ self.isInCrazyFlightmode = True
+
+ if (item == 0):
+ newState = False
+ else:
+ newState = True
+ self.maxThrust.setEnabled(newState)
+ self.maxAngle.setEnabled(newState)
+ self.minThrust.setEnabled(newState)
+ self.thrustLoweringSlewRateLimit.setEnabled(newState)
+ self.slewEnableLimit.setEnabled(newState)
+ self.maxYawRate.setEnabled(newState)
+
+ @pyqtSlot(bool)
+ def changeXmode(self, checked):
+ self.helper.cf.commander.set_client_xmode(checked)
+ Config().set("client_side_xmode", checked)
+ logger.info("Clientside X-mode enabled: %s", checked)
+
+ def alt1_updated(self, state):
+ if state:
+ self._ring_effect += 1
+ if self._ring_effect > self._ledring_nbr_effects:
+ self._ring_effect = 0
+ self.helper.cf.param.set_value("ring.effect", str(self._ring_effect))
+
+ def alt2_updated(self, state):
+ self.helper.cf.param.set_value("ring.headlightEnable", str(state))
+
+ def _ring_populate_dropdown(self):
+ try:
+ nbr = int(self.helper.cf.param.values["ring"]["neffect"])
+ current = int(self.helper.cf.param.values["ring"]["effect"])
+ except KeyError:
+ return
+
+ hardcoded_names = {0: "Off",
+ 1: "White spinner",
+ 2: "Color spinner",
+ 3: "Tilt effect",
+ 4: "Brightness effect",
+ 5: "Color spinner 2",
+ 6: "Double spinner",
+ 7: "Solid color effect",
+ 8: "Factory test",
+ 9: "Battery status",
+ 10: "Boat lights",
+ 11: "Alert",
+ 12: "Gravity"}
+
+ for i in range(nbr+1):
+ name = "{}: ".format(i)
+ if i in hardcoded_names:
+ name += hardcoded_names[i]
+ else:
+ name += "N/A"
+ self._led_ring_effect.addItem(name, QVariant(i))
+
+ self._led_ring_effect.setCurrentIndex(current)
+ self._led_ring_effect.currentIndexChanged.connect(self._ring_effect_changed)
+ self.helper.cf.param.add_update_callback(group="ring",
+ name="effect",
+ cb=self._ring_effect_updated)
+
+ def _ring_effect_changed(self, index):
+ i = self._led_ring_effect.itemData(index).toInt()[0]
+ logger.info("Changed effect to {}".format(i))
+ if i != self.helper.cf.param.values["ring"]["effect"]:
+ self.helper.cf.param.set_value("ring.effect", str(i))
+
+ def _ring_effect_updated(self, name, value):
+ self._led_ring_effect.setCurrentIndex(int(value))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..286da7cc9555d72815377df2eb0c6ba072978286
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/FlightTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..613350a11adab4b15795cc9c1e0bcd7416a9d08c
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.py
@@ -0,0 +1,295 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+This tab plots different logging data defined by configurations that has been
+pre-configured.
+"""
+import math
+
+__author__ = 'Bitcraze AB'
+__all__ = ['GpsTab']
+
+import glob
+import json
+import logging
+import os
+import sys
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+
+from pprint import pprint
+import datetime
+
+#from cfclient.ui.widgets.plotwidget import PlotWidget
+
+from cflib.crazyflie.log import Log, LogVariable, LogConfig
+
+from cfclient.ui.tab import Tab
+
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+
+try:
+ from PyKDE4.marble import *
+ should_enable_tab = True
+except:
+ should_enable_tab = False
+
+import sys
+
+gps_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/gpsTab.ui")[0]
+
+class GpsTab(Tab, gps_tab_class):
+ """Tab for plotting logging data"""
+
+ _log_data_signal = pyqtSignal(int, object, object)
+ _log_error_signal = pyqtSignal(object, str)
+
+ _disconnected_signal = pyqtSignal(str)
+ _connected_signal = pyqtSignal(str)
+ _console_signal = pyqtSignal(str)
+
+
+ def __init__(self, tabWidget, helper, *args):
+ super(GpsTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "GPS"
+ self.menuName = "GPS"
+
+ self.tabWidget = tabWidget
+ self.helper = helper
+ self._cf = helper.cf
+ self._got_home_point = False
+ self._line = ""
+
+ if not should_enable_tab:
+ self.enabled = False
+
+ if self.enabled:
+ # create the marble widget
+ #self._marble = Marble.MarbleWidget()
+ self._marble = FancyMarbleWidget()
+
+ # Load the OpenStreetMap map
+ self._marble.setMapThemeId("earth/openstreetmap/openstreetmap.dgml")
+
+ # Enable the cloud cover and enable the country borders
+ self._marble.setShowClouds(True)
+ self._marble.setShowBorders(True)
+
+ # Hide the FloatItems: Compass and StatusBar
+ self._marble.setShowOverviewMap(False)
+ self._marble.setShowScaleBar(False)
+ self._marble.setShowCompass(False)
+
+ self._marble.setShowGrid(False)
+ self._marble.setProjection(Marble.Mercator)
+
+ # Change the map to center on Australia
+
+ self._marble.zoomView(10)
+
+ # create the slider
+ self.zoomSlider = QSlider(Qt.Horizontal)
+
+ self._reset_max_btn.clicked.connect(self._reset_max)
+
+ # add all the components
+ #self.gpslayout.addWidget(self._marble)
+ self.map_layout.addWidget(self._marble)
+ # Connect the signals
+ self._log_data_signal.connect(self._log_data_received)
+ self._log_error_signal.connect(self._logging_error)
+ self._connected_signal.connect(self._connected)
+ self._disconnected_signal.connect(self._disconnected)
+
+ # Connect the callbacks from the Crazyflie API
+ self.helper.cf.disconnected.add_callback(
+ self._disconnected_signal.emit)
+ self.helper.cf.connected.add_callback(
+ self._connected_signal.emit)
+
+ else:
+ logger.warning("GPS tab not enabled since no Python"
+ "bindings for Marble was found")
+
+ self._max_speed = 0.0
+
+ self._fix_types = {
+ 0: "No fix",
+ 1: "Dead reckoning only",
+ 2: "2D-fix",
+ 3: "3D-fix",
+ 4: "GNSS+dead",
+ 5: "Time only fix"
+ }
+
+ def _connected(self, link_uri):
+ lg = LogConfig("GPS", 100)
+ lg.add_variable("gps.lat")
+ lg.add_variable("gps.lon")
+ lg.add_variable("gps.hMSL")
+ lg.add_variable("gps.heading")
+ lg.add_variable("gps.gSpeed")
+ lg.add_variable("gps.hAcc")
+ lg.add_variable("gps.fixType")
+ try:
+ self._cf.log.add_config(lg)
+ lg.data_received_cb.add_callback(self._log_data_signal.emit)
+ lg.error_cb.add_callback(self._log_error_signal.emit)
+ lg.start()
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError as e:
+ logger.warning(str(e))
+ self._max_speed = 0.0
+
+ def _disconnected(self, link_uri):
+ """Callback for when the Crazyflie has been disconnected"""
+ self._got_home_point = False
+ return
+
+ def _logging_error(self, log_conf, msg):
+ """Callback from the log layer when an error occurs"""
+ QMessageBox.about(self, "Plot error", "Error when starting log config"
+ " [%s]: %s" % (log_conf.name, msg))
+
+ def _reset_max(self):
+ """Callback from reset button"""
+ self._max_speed = 0.0
+ self._speed_max.setText(str(self._max_speed))
+ self._marble.clear_data()
+
+ self._long.setText("")
+ self._lat.setText("")
+ self._height.setText("")
+
+ self._speed.setText("")
+ self._heading.setText("")
+ self._accuracy.setText("")
+
+ self._fix_type.setText("")
+
+
+ def _log_data_received(self, timestamp, data, logconf):
+ """Callback when the log layer receives new data"""
+
+ long = float(data["gps.lon"])/10000000.0
+ lat = float(data["gps.lat"])/10000000.0
+ alt = float(data["gps.hMSL"])/1000.0
+ speed = float(data["gps.gSpeed"])/1000.0
+ accuracy = float(data["gps.hAcc"])/1000.0
+ fix_type = float(data["gps.fixType"])
+ heading = float(data["gps.heading"])
+
+ self._long.setText(str(long))
+ self._lat.setText(str(lat))
+ self._height.setText(str(alt))
+
+ self._speed.setText(str(speed))
+ self._heading.setText(str(heading))
+ self._accuracy.setText(str(accuracy))
+ if speed > self._max_speed:
+ self._max_speed = speed
+ self._speed_max.setText(str(self._max_speed))
+
+ self._fix_type.setText(self._fix_types[fix_type])
+
+
+ point = Marble.GeoDataCoordinates(long, lat, alt,
+ Marble.GeoDataCoordinates.Degree)
+ if not self._got_home_point:
+ self._got_home_point = True
+
+ self._marble.centerOn(point, True)
+ self._marble.zoomView(4000, Marble.Jump)
+
+ self._marble.add_data(long, lat, alt, accuracy,
+ True if fix_type == 3 else False)
+
+# If Marble is not installed then do not create MarbleWidget subclass
+if should_enable_tab:
+ class FancyMarbleWidget(Marble.MarbleWidget):
+ def __init__(self):
+ Marble.MarbleWidget.__init__(self)
+ self._points = []
+ self._lat = None
+ self._long = None
+ self._height = None
+ self._accu = None
+
+ def clear_data(self):
+ self._points = []
+ self._lat = None
+ self._long = None
+ self._height = None
+ self._accu = None
+
+ def add_data(self, long, lat, height, accu, locked):
+ self._points.append([long, lat, height, accu, locked])
+ self._lat = lat
+ self._long = long
+ self._height = height
+ self._accu = accu
+ self.update()
+
+ def customPaint(self, painter):
+ if self._lat:
+ current = Marble.GeoDataCoordinates(self._long,
+ self._lat,
+ self._height,
+ Marble.GeoDataCoordinates.Degree)
+
+ #Paint data points
+ for p in self._points:
+ pos = Marble.GeoDataCoordinates(p[0],
+ p[1],
+ p[2],
+ Marble.GeoDataCoordinates.Degree)
+ if p[4]:
+ painter.setPen(Qt.green)
+ else:
+ painter.setPen(Qt.red)
+ painter.drawEllipse(pos, 1, 1)
+
+ # Paint accuracy
+ painter.setPen(Qt.blue)
+ painter.setBrush(QtGui.QBrush(QtGui.QColor(0, 0, 255, 64)))
+ pixel_per_meter = self.radiusFromDistance(self.distance())/(6371.0*1000)
+ painter.drawEllipse(current, self._accu*pixel_per_meter, self._accu*pixel_per_meter, False)
+
+ #Paint Crazyflie
+ painter.setPen(Qt.black)
+ painter.setBrush(Qt.NoBrush)
+ painter.drawText(current, "Crazyflie")
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..b3788bae74fe8f0f52ba0fc1fd4c7819e244ff81
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/GpsTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..b51292e637d556673afa256e8d8c57084bdf4f70
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.py
@@ -0,0 +1,161 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+
+"""
+Basic tab to be able to set (and test) colors in the LED-ring.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LEDTab']
+
+import logging
+import sys
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtGui, uic
+from PyQt4.QtCore import pyqtSignal
+from PyQt4.QtGui import QColorDialog
+
+from cfclient.ui.tab import Tab
+
+from cflib.crazyflie.mem import MemoryElement
+
+led_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/ledTab.ui")[0]
+
+class LEDTab(Tab, led_tab_class):
+ """Tab for plotting logging data"""
+
+ _connected_signal = pyqtSignal(str)
+ _disconnected_signal = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(LEDTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "LED"
+ self.menuName = "LED tab"
+ self.tabWidget = tabWidget
+
+ self._helper = helper
+
+ # Always wrap callbacks from Crazyflie API though QT Signal/Slots
+ # to avoid manipulating the UI when rendering it
+ self._connected_signal.connect(self._connected)
+ self._disconnected_signal.connect(self._disconnected)
+
+ # Connect the Crazyflie API callbacks to the signals
+ self._helper.cf.connected.add_callback(
+ self._connected_signal.emit)
+
+ self._helper.cf.disconnected.add_callback(
+ self._disconnected_signal.emit)
+
+ self._btns = [self._u1,
+ self._u2,
+ self._u3,
+ self._u4,
+ self._u5,
+ self._u6,
+ self._u7,
+ self._u8,
+ self._u9,
+ self._u10,
+ self._u11,
+ self._u12]
+
+ self._intensity = 1
+
+ self._u1.clicked.connect(lambda: self._select(0))
+ self._u2.clicked.connect(lambda: self._select(1))
+ self._u3.clicked.connect(lambda: self._select(2))
+ self._u4.clicked.connect(lambda: self._select(3))
+ self._u5.clicked.connect(lambda: self._select(4))
+ self._u6.clicked.connect(lambda: self._select(5))
+ self._u7.clicked.connect(lambda: self._select(6))
+ self._u8.clicked.connect(lambda: self._select(7))
+ self._u9.clicked.connect(lambda: self._select(8))
+ self._u10.clicked.connect(lambda: self._select(9))
+ self._u11.clicked.connect(lambda: self._select(10))
+ self._u12.clicked.connect(lambda: self._select(11))
+
+ self._mem = None
+
+ self._intensity_slider.valueChanged.connect(self._intensity_change)
+ self._intensity_slider.valueChanged.connect(
+ self._intensity_spin.setValue)
+ self._intensity_spin.valueChanged.connect(
+ self._intensity_slider.setValue)
+
+ def _select(self, nbr):
+ col = QColorDialog()
+ col = QtGui.QColorDialog.getColor()
+ if col.isValid() and self._mem:
+ logger.info(col.red())
+ self._mem.leds[nbr].set(r=col.red(), g=col.green(), b=col.blue())
+ self.sender().setStyleSheet("background-color: rgb({},{},{})"
+ .format(col.red(), col.green(),
+ col.blue()))
+ self._write_led_output()
+
+ def _intensity_change(self, value):
+ self._intensity = value
+ self._write_led_output()
+
+ def _write_led_output(self):
+ if self._mem:
+ for led in self._mem.leds:
+ led.intensity = self._intensity
+ self._mem.write_data(self._led_write_done)
+ else:
+ logger.info("No LED-ring memory found!")
+
+ def _led_write_done(self, mem, addr):
+ logger.info("LED write done callback")
+
+ def _connected(self, link_uri):
+ """Callback when the Crazyflie has been connected"""
+ self._mem = self._helper.cf.mem.get_mems(
+ MemoryElement.TYPE_DRIVER_LED)[0]
+ logger.info(self._mem)
+ if self._mem:
+ for btn in self._btns:
+ btn.setEnabled(True)
+ btn.setStyleSheet("background-color: black")
+ self._intensity_slider.setEnabled(True)
+ self._intensity_spin.setEnabled(True)
+
+ def _disconnected(self, link_uri):
+ """Callback for when the Crazyflie has been disconnected"""
+ for btn in self._btns:
+ btn.setEnabled(False)
+ btn.setStyleSheet("background-color: none")
+ self._intensity_slider.setEnabled(False)
+ self._intensity_spin.setEnabled(False)
+ self._intensity_slider.setValue(100)
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..6687928d80f5b2fa0155ed7409e097da94404bfe
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LEDTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..7437823cab7d2dc454a34a2b0ae58a6c061e4547
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Shows all the parameters available in the Crazyflie and also gives the ability
+to edit them.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogBlockTab']
+
+import time
+import sys
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cfclient.ui.tab import Tab
+
+logblock_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/logBlockDebugTab.ui")[0]
+
+class LogBlockDebugTab(Tab, logblock_tab_class):
+ """
+ Used to show debug-information about log status.
+ """
+
+ _blocks_updated_signal = pyqtSignal(object, bool)
+ _disconnected_signal = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(LogBlockDebugTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Log Blocks Debugging"
+ self.menuName = "Log Blocks Debugging"
+
+ self._helper = helper
+ self.tabWidget = tabWidget
+
+ self._helper.cf.log.block_added_cb.add_callback(self._block_added)
+ self._disconnected_signal.connect(self._disconnected)
+ self._helper.cf.disconnected.add_callback(self._disconnected_signal.emit)
+ self._blocks_updated_signal.connect(self._update_tree)
+
+ self._block_tree.setHeaderLabels(['Id', 'Name', 'Period (ms)', 'Added', 'Started', 'Error', 'Contents'])
+ self._block_tree.sortItems(0, QtCore.Qt.AscendingOrder)
+
+ def _block_added(self, block):
+ """Callback when a new logblock has been created"""
+ block.added_cb.add_callback(self._blocks_updated_signal.emit)
+ block.started_cb.add_callback(self._blocks_updated_signal.emit)
+
+ def _update_tree(self, conf, value):
+ """Update the block tree"""
+ self._block_tree.clear()
+ for block in self._helper.cf.log.log_blocks:
+ item = QtGui.QTreeWidgetItem()
+ item.setFlags(Qt.ItemIsEnabled |
+ Qt.ItemIsSelectable)
+ item.setData(0, Qt.DisplayRole, block.id)
+ item.setData(1, Qt.EditRole, block.name)
+ item.setData(2, Qt.DisplayRole, (block.period_in_ms))
+ item.setData(3, Qt.DisplayRole, block.added)
+ item.setData(4, Qt.EditRole, block.started)
+ item.setData(5, Qt.EditRole, block.err_no)
+ for var in block.variables:
+ subItem = QtGui.QTreeWidgetItem()
+ subItem.setFlags(Qt.ItemIsEnabled |
+ Qt.ItemIsSelectable)
+ subItem.setData(6, Qt.EditRole, var.name)
+ item.addChild(subItem)
+
+ self._block_tree.addTopLevelItem(item)
+ self._block_tree.expandItem(item)
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected"""
+ self._block_tree.clear()
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..072ba320bb399515f2283559891157209c168abd
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockDebugTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..98eb6ce9bc43add8187efcbb26ce1162f6f20762
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.py
@@ -0,0 +1,345 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+This tab shows all log blocks that are registered and can be used to start the
+logging and also to write the logging data to file.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogBlockTab']
+
+import sys
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cfclient.ui.tab import Tab
+
+logblock_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/logBlockTab.ui")[0]
+
+import logging
+logger = logging.getLogger(__name__)
+
+from PyQt4.QtGui import QApplication, QStyledItemDelegate, QAbstractItemView
+from PyQt4.QtGui import QStyleOptionButton, QStyle
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import pyqtSlot, pyqtSignal, QThread
+from PyQt4.QtCore import QAbstractItemModel, QModelIndex, QString, QVariant
+
+from cfclient.utils.logdatawriter import LogWriter
+
+
+class LogBlockChildItem(object):
+ """Class that acts as a child in the tree and represents one variable in
+ a log block"""
+ def __init__(self, parent, name):
+ """Initialize the node"""
+ self.parent = parent
+ self.name = name
+
+ def child_count(self):
+ """Return the number of children this node has"""
+ return 0
+
+
+class LogBlockItem(object):
+ """Class that acts as a parent in the tree view and represents a complete
+ log block"""
+
+ def __init__(self, block, model, connected_ts):
+ """Initialize the parent node"""
+ super(LogBlockItem, self).__init__()
+ self._block = block
+ self.parent = None
+ self.children = []
+ self.name = block.name
+ self.id = block.id
+ self.period = block.period_in_ms
+ self._model = model
+ self._log_file_writer = LogWriter(block, connected_ts)
+
+ self._block.started_cb.add_callback(self._set_started)
+ self._block.added_cb.add_callback(self._set_added)
+ self._block.error_cb.add_callback(self._log_error)
+
+ self._var_list = ""
+
+ for b in block.variables:
+ self.children.append(LogBlockChildItem(self, b.name))
+ self._var_list += "%s/" % b.name
+ self._var_list = self._var_list[:-1]
+
+ self._block_started = False
+ self._doing_transaction = False
+
+ def _log_error(self, logconfig, msg):
+ """Callback when there's an error starting the block in the Crazyflie"""
+ # Do nothing here, a pop-up will notify the user that the
+ # starting failed
+ self._doing_transaction = False
+
+ def _set_started(self, conf, started):
+ """Callback when a block has been started in the Crazyflie"""
+ logger.debug("%s started: %s", self.name, started)
+ if started:
+ self._block_started = True
+ else:
+ self._block_started = False
+ self._doing_transaction = False
+ self._model.refresh()
+
+ def logging_started(self):
+ """Return True if the block has been started, otherwise False"""
+ return self._block_started
+
+ def writing_to_file(self):
+ """Return True if the block is being logged to file, otherwise False"""
+ return self._log_file_writer.writing()
+
+ def start_writing_to_file(self):
+ """Start logging to file for this block"""
+ self._log_file_writer.start()
+
+ def stop_writing_to_file(self):
+ """Stop logging to file for this block"""
+ self._log_file_writer.stop()
+
+ def start(self):
+ """Start the logging of this block"""
+ self._doing_transaction = True
+ self._block.start()
+
+ def stop(self):
+ """Stop the logging of this block"""
+ self._doing_transaction = True
+ self._block.delete()
+
+ def doing_transaction(self):
+ """Return True if a block is being added or started, False when it's
+ been added/started/failed"""
+ return self._doing_transaction
+
+ def _set_added(self, conf, started):
+ """Callback when a block has been added to the Crazyflie"""
+ logger.debug("%s added: %s", self.name, started)
+
+ def var_list(self):
+ """Return a string containing all the variable names of the children"""
+ return self._var_list
+
+ def child_count(self):
+ """Return the number of children this node has"""
+ return len(self.children)
+
+ def get_child(self, index):
+ return self.children[index]
+
+
+class LogBlockModel(QAbstractItemModel):
+ def __init__(self, view, parent=None):
+ super(LogBlockModel, self).__init__(parent)
+ self._nodes = []
+ self._column_headers = ['Id', 'Name', 'Period (ms)', 'Start',
+ 'Write to file', 'Contents']
+ self._view = view
+ self._nodes_written_to_file = []
+
+ def add_block(self, block, connected_ts):
+ self._nodes.append(LogBlockItem(block, self, connected_ts))
+ self.layoutChanged.emit()
+
+ def refresh(self):
+ """Force a refresh of the view though the model"""
+ self.layoutChanged.emit()
+
+ def clicked(self, index):
+ """Callback when a cell has been clicked (mouse down/up on same cell)"""
+ node = index.internalPointer()
+ if not node.parent and index.column() == 3:
+ if node.logging_started():
+ node.stop()
+ else:
+ node.start()
+ if not node.parent and index.column() == 4:
+ if node.writing_to_file():
+ node.stop_writing_to_file()
+ else:
+ node.start_writing_to_file()
+ self.layoutChanged.emit()
+
+ def parent(self, index):
+ """Re-implemented method to get the parent of the given index"""
+ if not index.isValid():
+ return QModelIndex()
+
+ node = index.internalPointer()
+ if node.parent is None:
+ return QModelIndex()
+ else:
+ return self.createIndex(self._nodes.index(node.parent), 0,
+ node.parent)
+
+ def remove_block(self, block):
+ """Remove a block from the view"""
+ raise NotImplementedError()
+
+ def columnCount(self, parent):
+ """Re-implemented method to get the number of columns"""
+ return len(self._column_headers)
+
+ def headerData(self, section, orientation, role):
+ """Re-implemented method to get the headers"""
+ if role == Qt.DisplayRole:
+ return QString(self._column_headers[section])
+
+ def rowCount(self, parent):
+ """Re-implemented method to get the number of rows for a given index"""
+ parent_item = parent.internalPointer()
+ if parent.isValid():
+ parent_item = parent.internalPointer()
+ return parent_item.child_count()
+ else:
+ return len(self._nodes)
+
+ def index(self, row, column, parent):
+ """Re-implemented method to get the index for a specified
+ row/column/parent combination"""
+ if not self._nodes:
+ return QModelIndex()
+ node = parent.internalPointer()
+ if not node:
+ index = self.createIndex(row, column, self._nodes[row])
+ self._nodes[row].index = index
+ return index
+ else:
+ return self.createIndex(row, column, node.get_child(row))
+
+ def data(self, index, role):
+ """Re-implemented method to get the data for a given index and role"""
+ node = index.internalPointer()
+ parent = node.parent
+ if parent:
+ if role == Qt.DisplayRole and index.column() == 5:
+ return node.name
+ elif not parent and role == Qt.DisplayRole and index.column() == 5:
+ return node.var_list()
+ elif not parent and role == Qt.DisplayRole:
+ if index.column() == 0:
+ return node.id
+ if index.column() == 1:
+ return node.name
+ if index.column() == 2:
+ return str(node.period)
+ if role == Qt.TextAlignmentRole and \
+ (index.column() == 4 or index.column() == 3):
+ return Qt.AlignHCenter | Qt.AlignVCenter
+
+ return QVariant()
+
+ def reset(self):
+ """Reset the model"""
+ # Stop the logging to file
+ for node in self._nodes:
+ if node.writing_to_file():
+ node.stop_writing_to_file()
+ self._nodes = []
+ self.layoutChanged.emit()
+
+
+class CheckboxDelegate(QStyledItemDelegate):
+ """Custom delegate for rending checkboxes in the table"""
+
+ def paint(self, painter, option, index):
+ """Re-implemented paint method"""
+ item = index.internalPointer()
+ col = index.column()
+ if not item.parent and (col == 3 or col == 4):
+ s = QStyleOptionButton()
+ checkbox_rect = QApplication.style().\
+ subElementRect(QStyle.SE_CheckBoxIndicator, option)
+ s.rect = option.rect
+ center_offset = s.rect.width() / 2 - checkbox_rect.width() / 2
+ s.rect.adjust(center_offset, 0, 0, 0)
+
+ if col == 3:
+ if not item.doing_transaction():
+ s.state = QStyle.State_Enabled
+ if item.logging_started():
+ s.state |= QStyle.State_On
+
+ if col == 4:
+ s.state = QStyle.State_Enabled
+ if item.writing_to_file():
+ s.state |= QStyle.State_On
+
+ QApplication.style().drawControl(
+ QStyle.CE_CheckBox, s, painter)
+
+ else:
+ super(CheckboxDelegate, self).paint(painter, option, index)
+
+
+class LogBlockTab(Tab, logblock_tab_class):
+ """
+ Used to show debug-information about logblock status.
+ """
+
+ _blocks_updated_signal = pyqtSignal(bool)
+ _disconnected_signal = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ """Initialize the tab"""
+ super(LogBlockTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Log Blocks"
+ self.menuName = "Log Blocks"
+
+ self._helper = helper
+ self.tabWidget = tabWidget
+
+ self._helper.cf.log.block_added_cb.add_callback(self._block_added)
+ self._disconnected_signal.connect(self._disconnected)
+ self._helper.cf.disconnected.add_callback(
+ self._disconnected_signal.emit)
+
+ self._model = LogBlockModel(self._block_tree)
+ self._block_tree.setModel(self._model)
+ self._block_tree.clicked.connect(self._model.clicked)
+ self._block_tree.setItemDelegate(CheckboxDelegate())
+ self._block_tree.setSelectionMode(QAbstractItemView.NoSelection)
+
+ def _block_added(self, block):
+ """Callback from logging layer when a new block is added"""
+ self._model.add_block(block, self._helper.cf.connected_ts)
+
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected"""
+ self._model.reset()
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..c71e399d654417ad5df9fb3c9cbc87ec96f9b9b3
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogBlockTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..629b452f1a58161984140259d7bcb45ca3a24f08
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.py
@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Shows the Log TOC of available variables in the Crazyflie.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogTab']
+
+import time
+import sys
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+from cflib.crazyflie import Crazyflie
+
+from cfclient.ui.tab import Tab
+
+param_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/logTab.ui")[0]
+
+
+class LogTab(Tab, param_tab_class):
+ connectedSignal = pyqtSignal(str)
+ disconnectedSignal = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ super(LogTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Log TOC"
+ self.menuName = "Log TOC"
+
+ self.helper = helper
+ self.tabWidget = tabWidget
+
+ self.cf = helper.cf
+
+ # Init the tree widget
+ self.logTree.setHeaderLabels(['Name', 'ID', 'Unpack', 'Storage'])
+
+ self.cf.connected.add_callback(self.connectedSignal.emit)
+ self.connectedSignal.connect(self.connected)
+
+ # Clear the log TOC list when the Crazyflie is disconnected
+ self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
+ self.disconnectedSignal.connect(self.disconnected)
+
+ @pyqtSlot('QString')
+ def disconnected(self, linkname):
+ self.logTree.clear()
+
+ @pyqtSlot(str)
+ def connected(self, linkURI):
+ self.logTree.clear()
+
+ toc = self.cf.log.toc
+
+ for group in toc.toc.keys():
+ groupItem = QtGui.QTreeWidgetItem()
+ groupItem.setData(0, Qt.DisplayRole, group)
+ for param in toc.toc[group].keys():
+ item = QtGui.QTreeWidgetItem()
+ item.setData(0, Qt.DisplayRole, param)
+ item.setData(1, Qt.DisplayRole, toc.toc[group][param].ident)
+ item.setData(2, Qt.DisplayRole, toc.toc[group][param].pytype)
+ item.setData(3, Qt.DisplayRole, toc.toc[group][param].ctype)
+ groupItem.addChild(item)
+
+ self.logTree.addTopLevelItem(groupItem)
+ self.logTree.expandItem(groupItem)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..e9ffc7219c6f8354eb9c3150ad53016588c9790b
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/LogTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..7ac7fada4186aef56ba629f8f2ca53a44cc50369
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.py
@@ -0,0 +1,255 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Shows all the parameters available in the Crazyflie and also gives the ability
+to edit them.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ParamTab']
+
+import sys
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+from PyQt4.QtCore import QAbstractItemModel, QModelIndex, QString, QVariant
+from PyQt4.QtGui import QApplication, QStyledItemDelegate, QAbstractItemView, QBrush, QColor
+from PyQt4.QtGui import QSortFilterProxyModel
+
+from cfclient.ui.tab import Tab
+
+param_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/paramTab.ui")[0]
+
+import logging
+logger = logging.getLogger(__name__)
+
+class ParamChildItem(object):
+ """Represents a leaf-node in the tree-view (one parameter)"""
+ def __init__(self, parent, name, crazyflie):
+ """Initialize the node"""
+ self.parent = parent
+ self.name = name
+ self.ctype = None
+ self.access = None
+ self.value = ""
+ self._cf = crazyflie
+ self.is_updating = True
+
+ def updated(self, name, value):
+ """Callback from the param layer when a parameter has been updated"""
+ self.value = value
+ self.is_updating = False
+ self.parent.model.refresh()
+
+ def set_value(self, value):
+ """Send the update value to the Crazyflie. It will automatically be
+ read again after sending and then the updated callback will be
+ called"""
+ complete_name = "%s.%s" % (self.parent.name, self.name)
+ self._cf.param.set_value(complete_name, value)
+ self.is_updating = True
+
+ def child_count(self):
+ """Return the number of children this node has"""
+ return 0
+
+
+class ParamGroupItem(object):
+ """Represents a parameter group in the tree-view"""
+ def __init__(self, name, model):
+ """Initialize the parent node"""
+ super(ParamGroupItem, self).__init__()
+ self.parent = None
+ self.children = []
+ self.name = name
+ self.model = model
+
+ def child_count(self):
+ """Return the number of children this node has"""
+ return len(self.children)
+
+class ParamBlockModel(QAbstractItemModel):
+ """Model for handling the parameters in the tree-view"""
+ def __init__(self, parent):
+ """Create the empty model"""
+ super(ParamBlockModel, self).__init__(parent)
+ self._nodes = []
+ self._column_headers = ['Name', 'Type', 'Access', 'Value']
+ self._red_brush = QBrush(QColor("red"))
+
+ def set_toc(self, toc, crazyflie):
+ """Populate the model with data from the param TOC"""
+
+ # No luck using proxy sorting, so do it here instead...
+ for group in sorted(toc.keys()):
+ new_group = ParamGroupItem(group, self)
+ for param in sorted(toc[group].keys()):
+ new_param = ParamChildItem(new_group, param, crazyflie)
+ new_param.ctype = toc[group][param].ctype
+ new_param.access = toc[group][param].get_readable_access()
+ crazyflie.param.add_update_callback(
+ group=group, name=param, cb=new_param.updated)
+ new_group.children.append(new_param)
+ self._nodes.append(new_group)
+
+ self.layoutChanged.emit()
+
+ def refresh(self):
+ """Force a refresh of the view though the model"""
+ self.layoutChanged.emit()
+
+ def parent(self, index):
+ """Re-implemented method to get the parent of the given index"""
+ if not index.isValid():
+ return QModelIndex()
+
+ node = index.internalPointer()
+ if node.parent is None:
+ return QModelIndex()
+ else:
+ return self.createIndex(self._nodes.index(node.parent), 0,
+ node.parent)
+
+ def columnCount(self, parent):
+ """Re-implemented method to get the number of columns"""
+ return len(self._column_headers)
+
+ def headerData(self, section, orientation, role):
+ """Re-implemented method to get the headers"""
+ if role == Qt.DisplayRole:
+ return QString(self._column_headers[section])
+
+ def rowCount(self, parent):
+ """Re-implemented method to get the number of rows for a given index"""
+ parent_item = parent.internalPointer()
+ if parent.isValid():
+ parent_item = parent.internalPointer()
+ return parent_item.child_count()
+ else:
+ return len(self._nodes)
+
+ def index(self, row, column, parent):
+ """Re-implemented method to get the index for a specified
+ row/column/parent combination"""
+ if not self._nodes:
+ return QModelIndex()
+ node = parent.internalPointer()
+ if not node:
+ index = self.createIndex(row, column, self._nodes[row])
+ self._nodes[row].index = index
+ return index
+ else:
+ return self.createIndex(row, column, node.children[row])
+
+ def data(self, index, role):
+ """Re-implemented method to get the data for a given index and role"""
+ node = index.internalPointer()
+ parent = node.parent
+ if not parent:
+ if role == Qt.DisplayRole and index.column() == 0:
+ return node.name
+ elif role == Qt.DisplayRole:
+ if index.column() == 0:
+ return node.name
+ if index.column() == 1:
+ return node.ctype
+ if index.column() == 2:
+ return node.access
+ if index.column() == 3:
+ return node.value
+ elif role == Qt.EditRole and index.column() == 3:
+ return node.value
+ elif (role == Qt.BackgroundRole and index.column() == 3
+ and node.is_updating):
+ return self._red_brush
+
+ return QVariant()
+
+ def setData(self, index, value, role):
+ """Re-implemented function called when a value has been edited"""
+ node = index.internalPointer()
+ if role == Qt.EditRole:
+ new_val = str(value.toString())
+ # This will not update the value, only trigger a setting and
+ # reading of the parameter from the Crazyflie
+ node.set_value(new_val)
+ return True
+ return False
+
+
+ def flags(self, index):
+ """Re-implemented function for getting the flags for a certain index"""
+ flag = super(ParamBlockModel, self).flags(index)
+ node = index.internalPointer()
+ if index.column() == 3 and node.parent and node.access=="RW":
+ flag |= Qt.ItemIsEditable
+ return flag
+
+
+ def reset(self):
+ """Reset the model"""
+ self._nodes = []
+ super(ParamBlockModel, self).reset()
+ self.layoutChanged.emit()
+
+
+class ParamTab(Tab, param_tab_class):
+ """
+ Show all the parameters in the TOC and give the user the ability to edit
+ them
+ """
+ _expand_all_signal = pyqtSignal()
+ _connected_signal = pyqtSignal(str)
+ _disconnected_signal = pyqtSignal(str)
+
+ def __init__(self, tabWidget, helper, *args):
+ """Create the parameter tab"""
+ super(ParamTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Parameters"
+ self.menuName = "Parameters"
+
+ self.helper = helper
+ self.tabWidget = tabWidget
+ self.cf = helper.cf
+
+ self.cf.connected.add_callback(self._connected_signal.emit)
+ self._connected_signal.connect(self._connected)
+ self.cf.disconnected.add_callback(self._disconnected_signal.emit)
+ self._disconnected_signal.connect(self._disconnected)
+
+ self._model = ParamBlockModel(None)
+ self.paramTree.setModel(self._model)
+
+ def _connected(self, link_uri):
+ self._model.set_toc(self.cf.param.toc.toc, self.helper.cf)
+ self.paramTree.expandAll()
+
+ def _disconnected(self, link_uri):
+ self._model.reset()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..68c20f7541dca74cae5f9e4f4c2fd3595f495658
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ParamTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.py
new file mode 100755
index 0000000000000000000000000000000000000000..b6cd3e19caa2d8e6550f269887ba663274fc042f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.py
@@ -0,0 +1,255 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+This tab plots different logging data defined by configurations that has been
+pre-configured.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PlotTab']
+
+import glob
+import json
+import logging
+import os
+import sys
+
+logger = logging.getLogger(__name__)
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import pyqtSlot, pyqtSignal, QThread, Qt
+from PyQt4.QtGui import QMessageBox
+from PyQt4.QtGui import QApplication, QStyledItemDelegate, QAbstractItemView
+from PyQt4.QtCore import QAbstractItemModel, QModelIndex, QString, QVariant
+
+from pprint import pprint
+import datetime
+
+from cfclient.ui.widgets.plotwidget import PlotWidget
+
+from cflib.crazyflie.log import Log
+
+from cfclient.ui.tab import Tab
+
+plot_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/tabs/plotTab.ui")[0]
+
+class LogConfigModel(QAbstractItemModel):
+ """Model for log configurations in the ComboBox"""
+ def __init__(self, parent=None):
+ super(LogConfigModel, self).__init__(parent)
+ self._nodes = []
+
+ def add_block(self, block):
+ self._nodes.append(block)
+ self.layoutChanged.emit()
+
+ def parent(self, index):
+ """Re-implemented method to get the parent of the given index"""
+ return QModelIndex()
+
+ def remove_block(self, block):
+ """Remove a block from the view"""
+ raise NotImplementedError()
+
+ def columnCount(self, parent):
+ """Re-implemented method to get the number of columns"""
+ return 1
+
+ def rowCount(self, parent):
+ """Re-implemented method to get the number of rows for a given index"""
+ parent_item = parent.internalPointer()
+ if parent.isValid():
+ parent_item = parent.internalPointer()
+ return 0
+ else:
+ return len(self._nodes)
+
+ def index(self, row, column, parent):
+ """Re-implemented method to get the index for a specified
+ row/column/parent combination"""
+ if not self._nodes:
+ return QModelIndex()
+ node = parent.internalPointer()
+ if not node:
+ index = self.createIndex(row, column, self._nodes[row])
+ return index
+ else:
+ return self.createIndex(row, column, node.get_child(row))
+
+ def data(self, index, role):
+ """Re-implemented method to get the data for a given index and role"""
+ node = index.internalPointer()
+ if not index.isValid() or not 0 <= index.row() < len(self._nodes):
+ return QVariant()
+ if role == Qt.DisplayRole:
+ return self._nodes[index.row()].name
+ return QVariant()
+
+ def reset(self):
+ """Reset the model"""
+ self._nodes = []
+ self.layoutChanged.emit()
+
+ def get_config(self, i):
+ return self._nodes[i]
+
+class PlotTab(Tab, plot_tab_class):
+ """Tab for plotting logging data"""
+
+ _log_data_signal = pyqtSignal(int, object, object)
+ _log_error_signal = pyqtSignal(object, str)
+ _disconnected_signal = pyqtSignal(str)
+ _connected_signal = pyqtSignal(str)
+
+ colors = ['g', 'b', 'm', 'r', 'y', 'c']
+
+ def __init__(self, tabWidget, helper, *args):
+ super(PlotTab, self).__init__(*args)
+ self.setupUi(self)
+
+ self.tabName = "Plotter"
+ self.menuName = "Plotter"
+
+ self._log_error_signal.connect(self._logging_error)
+
+ self._plot = PlotWidget(fps=30)
+ # Check if we could find the PyQtImport. If not, then
+ # set this tab as disabled
+ self.enabled = self._plot.can_enable
+
+ self._model = LogConfigModel()
+ self.dataSelector.setModel(self._model)
+ self._log_data_signal.connect(self._log_data_received)
+ self.tabWidget = tabWidget
+ self.helper = helper
+ self.plotLayout.addWidget(self._plot)
+
+ # Connect external signals if we can use the tab
+ if self.enabled:
+ self._disconnected_signal.connect(self._disconnected)
+ self.helper.cf.disconnected.add_callback(
+ self._disconnected_signal.emit)
+
+ self._connected_signal.connect(self._connected)
+ self.helper.cf.connected.add_callback(
+ self._connected_signal.emit)
+
+ self.helper.cf.log.block_added_cb.add_callback(self._config_added)
+ self.dataSelector.currentIndexChanged.connect(
+ self._selection_changed)
+
+ self._previous_config = None
+ self._started_previous = False
+
+ def _connected(self, link_uri):
+ """Callback when the Crazyflie has been connected"""
+ self._plot.removeAllDatasets()
+ self._plot.set_title("")
+
+ def _disconnected(self, link_uri):
+ """Callback for when the Crazyflie has been disconnected"""
+ self._model.reset()
+ self.dataSelector.setCurrentIndex(-1)
+ self._previous_config = None
+ self._started_previous = False
+
+ def _log_data_signal_wrapper(self, ts, data, logconf):
+ """Wrapper for signal"""
+
+ # For some reason the *.emit functions are not
+ # the same over time (?!) so they cannot be registered and then
+ # removed as callbacks.
+ self._log_data_signal.emit(ts, data, logconf)
+
+ def _log_error_signal_wrapper(self, config, msg):
+ """Wrapper for signal"""
+
+ # For some reason the *.emit functions are not
+ # the same over time (?!) so they cannot be registered and then
+ # removed as callbacks.
+ self._log_error_signal.emit(config, msg)
+
+ def _selection_changed(self, i):
+ """Callback from ComboBox when a new item has been selected"""
+
+ # Check if we have disconnected
+ if i < 0:
+ return
+ # First check if we need to stop the old block
+ if self._started_previous and self._previous_config:
+ logger.debug("Should stop config [%s], stopping!",
+ self._previous_config.name)
+ self._previous_config.delete()
+
+ # Remove our callback for the previous config
+ if self._previous_config:
+ self._previous_config.data_received_cb.remove_callback(
+ self._log_data_signal_wrapper)
+ self._previous_config.error_cb.remove_callback(
+ self._log_error_signal_wrapper)
+
+ lg = self._model.get_config(i)
+ if not lg.started:
+ logger.debug("Config [%s] not started, starting!", lg.name)
+ self._started_previous = True
+ lg.start()
+ else:
+ self._started_previous = False
+ self._plot.removeAllDatasets()
+ color_selector = 0
+
+ self._plot.set_title(lg.name)
+
+ for d in lg.variables:
+ self._plot.add_curve(d.name,
+ self.colors[color_selector % len(self.colors)])
+ color_selector += 1
+ lg.data_received_cb.add_callback(self._log_data_signal_wrapper)
+ lg.error_cb.add_callback(self._log_error_signal_wrapper)
+
+ self._previous_config = lg
+
+ def _config_added(self, logconfig):
+ """Callback from the log layer when a new config has been added"""
+ logger.debug("Callback for new config [%s]", logconfig.name)
+ self._model.add_block(logconfig)
+
+ def _logging_error(self, log_conf, msg):
+ """Callback from the log layer when an error occurs"""
+ QMessageBox.about(self, "Plot error", "Error when starting log config"
+ " [%s]: %s" % (log_conf.name, msg))
+
+ def _log_data_received(self, timestamp, data, logconf):
+ """Callback when the log layer receives new data"""
+
+ # Check so that the incoming data belongs to what we are currently
+ # logging
+ if self._previous_config:
+ if self._previous_config.name == logconf.name:
+ self._plot.add_data(data, timestamp)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..6177d2525e46ab08df538999f586baf6b8b331ad
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/PlotTab.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..e9bfba630fe318d76e306b0a1a50053891724d05
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.py
@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Find all the available tabs so they can be loaded.
+
+Dropping a new .py file into this directory will automatically list and load
+it into the UI when it is started.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = []
+
+import os
+import glob
+import logging
+
+logger = logging.getLogger(__name__)
+
+found_tabs = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) + "/[A-Za-z]*Tab.py")]
+if len(found_tabs) == 0:
+ found_tabs = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) +
+ "/[A-Za-z]*Tab.pyc")]
+
+logger.debug("Found tabs: %s", found_tabs)
+
+available = []
+
+for tab in found_tabs:
+ tabModule = __import__(tab, globals(), locals(), [tab], -1)
+ available.append(getattr(tabModule, tab))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..229ceb2b231e511d54ef38e37ed32371bd1adde2
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/consoleTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/consoleTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..39f514a81cba1599746ce01e0a742fa19a281128
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/consoleTab.ui
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>693</width>
+ <height>463</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QTextEdit" name="console"/>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/exampleTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/exampleTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..a53e2ffac4ef3a32acd60be864b82ab4b461dc29
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/exampleTab.ui
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>765</width>
+ <height>384</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Plot</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/flightTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/flightTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..5653fb7337ddcaa21f140c0fc3942f01e5e13ecf
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/flightTab.ui
@@ -0,0 +1,678 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1228</width>
+ <height>916</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QSplitter" name="splitter_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetMaximumSize</enum>
+ </property>
+ <item>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>320</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>Basic Flight Control</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="1" column="1">
+ <widget class="QComboBox" name="flightModeCombo">
+ <property name="minimumSize">
+ <size>
+ <width>150</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>150</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="toolTip">
+ <string><html><head/><body><p>Select what flightmode to use:</p><p> * Safe prevents crashing</p><p> * Crazy does not prevent crashing :)</p></body></html></string>
+ </property>
+ <property name="editable">
+ <bool>false</bool>
+ </property>
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <item>
+ <property name="text">
+ <string>Normal</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Advanced</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Maximum</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Thrust mode</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QComboBox" name="thrustModeCombo">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <item>
+ <property name="text">
+ <string>Linear</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Quadratic</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Roll Trim</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="targetCalRoll">
+ <property name="minimum">
+ <double>-20.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>20.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.200000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QDoubleSpinBox" name="targetCalPitch">
+ <property name="minimum">
+ <double>-20.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>20.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.200000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Pitch Trim</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="minimumSize">
+ <size>
+ <width>100</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Flight mode</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QCheckBox" name="crazyflieXModeCheckbox">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Crazyflie X-mode</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QCheckBox" name="clientXModeCheckbox">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Client X-mode</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QRadioButton" name="angularPidRadioButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Attitude control</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QRadioButton" name="ratePidRadioButton">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Rate control</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>320</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>Advanced Flight Control</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Max thrust (%)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="layoutDirection">
+ <enum>Qt::LeftToRight</enum>
+ </property>
+ <property name="text">
+ <string>Max angle/rate</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Min thrust (%)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_26">
+ <property name="text">
+ <string>Thrust lowering slewrate (%/sec)</string>
+ </property>
+ <property name="wordWrap">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_31">
+ <property name="text">
+ <string>SlewLimit (%)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_32">
+ <property name="text">
+ <string>Max Yaw angle/rate</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="maxThrust"/>
+ </item>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="minThrust"/>
+ </item>
+ <item row="4" column="1">
+ <widget class="QDoubleSpinBox" name="slewEnableLimit"/>
+ </item>
+ <item row="5" column="1">
+ <widget class="QDoubleSpinBox" name="thrustLoweringSlewRateLimit"/>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSpinBox" name="maxYawRate">
+ <property name="minimum">
+ <number>0</number>
+ </property>
+ <property name="maximum">
+ <number>500</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSpinBox" name="maxAngle">
+ <property name="minimum">
+ <number>0</number>
+ </property>
+ <property name="maximum">
+ <number>500</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="minimumSize">
+ <size>
+ <width>320</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>Expansion boards</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="1">
+ <widget class="QComboBox" name="_led_ring_effect">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>LED-ring effect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QCheckBox" name="_led_ring_headlight">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>LED-ring headlight</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetDefaultConstraint</enum>
+ </property>
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>2</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Flight Data</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QSplitter" name="splitter">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QVBoxLayout" name="verticalLayout_4"/>
+ </widget>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QGridLayout" name="gridLayout_5" rowstretch="0,3,3,3,3,3" rowminimumheight="1,1,1,1,1,1">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetMinimumSize</enum>
+ </property>
+ <property name="horizontalSpacing">
+ <number>6</number>
+ </property>
+ <property name="verticalSpacing">
+ <number>0</number>
+ </property>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_17">
+ <property name="text">
+ <string>ASL</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QLineEdit" name="actualPitch"/>
+ </item>
+ <item row="1" column="8" rowspan="5">
+ <widget class="QProgressBar" name="actualM1">
+ <property name="maximum">
+ <number>65535</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="10" rowspan="5">
+ <widget class="QProgressBar" name="actualM3">
+ <property name="maximum">
+ <number>65535</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Thrust</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="actualASL">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_14">
+ <property name="text">
+ <string>Target</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="targetRoll"/>
+ </item>
+ <item row="1" column="6" rowspan="5">
+ <widget class="QProgressBar" name="thrustProgress">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Maximum" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="sizeIncrement">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximum">
+ <number>65600</number>
+ </property>
+ <property name="value">
+ <number>24</number>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>Thrust</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="8">
+ <widget class="QLabel" name="M1label">
+ <property name="text">
+ <string>M1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>Roll</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="10">
+ <widget class="QLabel" name="M3label">
+ <property name="text">
+ <string>M3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="targetYaw"/>
+ </item>
+ <item row="1" column="9" rowspan="5">
+ <widget class="QProgressBar" name="actualM2">
+ <property name="maximum">
+ <number>65535</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="targetPitch"/>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>Pitch</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_15">
+ <property name="text">
+ <string>Actual</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="actualYaw"/>
+ </item>
+ <item row="0" column="9">
+ <widget class="QLabel" name="M2label">
+ <property name="text">
+ <string>M2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="11" rowspan="5">
+ <widget class="QProgressBar" name="actualM4">
+ <property name="maximum">
+ <number>65535</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="targetThrust"/>
+ </item>
+ <item row="0" column="11">
+ <widget class="QLabel" name="M4label">
+ <property name="text">
+ <string>M4</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLineEdit" name="actualThrust"/>
+ </item>
+ <item row="0" column="12" rowspan="6">
+ <layout class="QGridLayout" name="gridLayout_7"/>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>Yaw</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="actualRoll"/>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="targetASL">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/gpsTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/gpsTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..da2c503d7a5a80bb3513cbe47bd12390c6291dd8
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/gpsTab.ui
@@ -0,0 +1,160 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>765</width>
+ <height>384</height>
+ </rect>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Ignored">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="windowTitle">
+ <string>Plot</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="gpslayout" stretch="0,0">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetDefaultConstraint</enum>
+ </property>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetDefaultConstraint</enum>
+ </property>
+ <item row="0" column="5">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Accuracy</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="_lat">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Lat</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Long</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLineEdit" name="_speed_max">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="5">
+ <widget class="QLineEdit" name="_accuracy"/>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="_fix_type">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Ground speed</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLineEdit" name="_height">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLineEdit" name="_speed">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="_heading">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Height (MSL)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="_long">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Heading</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="_reset_max_btn">
+ <property name="text">
+ <string>Reset all</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QWidget" name="widget" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="map_layout"/>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ledTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ledTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..83dc36924844bc233c793dbf093dc34a671a76df
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/ledTab.ui
@@ -0,0 +1,336 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>765</width>
+ <height>392</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Plot</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <spacer name="horizontalSpacer">
+ <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>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="3" column="0">
+ <widget class="QPushButton" name="_u4">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="autoFillBackground">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>U4</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="6">
+ <widget class="QPushButton" name="_u10">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U10</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="_u1">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="_u7">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U7</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="_u3">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="_u2">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QPushButton" name="_u12">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U12</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="QPushButton" name="_u11">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U11</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QPushButton" name="_u5">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U5</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QPushButton" name="_u6">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U6</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QPushButton" name="_u8">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U8</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="5">
+ <widget class="QPushButton" name="_u9">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>5</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>U9</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="0">
+ <widget class="QSlider" name="_intensity_slider">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimum">
+ <number>1</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::NoTicks</enum>
+ </property>
+ <property name="tickInterval">
+ <number>10</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="1" column="0">
+ <widget class="QSpinBox" name="_intensity_spin">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="suffix">
+ <string>%</string>
+ </property>
+ <property name="prefix">
+ <string/>
+ </property>
+ <property name="minimum">
+ <number>1</number>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Intensity</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <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>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockDebugTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockDebugTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..8c993df17c85934adf0cb424465c447cdfa222bc
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockDebugTab.ui
@@ -0,0 +1,75 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>668</width>
+ <height>528</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTreeWidget" name="_block_tree">
+ <property name="sortingEnabled">
+ <bool>true</bool>
+ </property>
+ <property name="columnCount">
+ <number>7</number>
+ </property>
+ <attribute name="headerDefaultSectionSize">
+ <number>100</number>
+ </attribute>
+ <attribute name="headerHighlightSections">
+ <bool>false</bool>
+ </attribute>
+ <attribute name="headerShowSortIndicator" stdset="0">
+ <bool>true</bool>
+ </attribute>
+ <column>
+ <property name="text">
+ <string notr="true">1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">2</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">3</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">4</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>5</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>6</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>7</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..a058deb67998a606a1a97b55d74bc6cd05518e46
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logBlockTab.ui
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>668</width>
+ <height>528</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTreeView" name="_block_tree">
+ <property name="sortingEnabled">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..08a68ca0da5f720305f9298a5943aed639c82cae
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/logTab.ui
@@ -0,0 +1,60 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>668</width>
+ <height>528</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTreeWidget" name="logTree">
+ <property name="columnCount">
+ <number>4</number>
+ </property>
+ <attribute name="headerDefaultSectionSize">
+ <number>100</number>
+ </attribute>
+ <attribute name="headerHighlightSections">
+ <bool>false</bool>
+ </attribute>
+ <attribute name="headerShowSortIndicator" stdset="0">
+ <bool>false</bool>
+ </attribute>
+ <column>
+ <property name="text">
+ <string notr="true">1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">2</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">3</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">4</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout"/>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/paramTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/paramTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..6b732b404025bec6d9938a0dc8875a8fb564bf50
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/paramTab.ui
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>668</width>
+ <height>528</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTreeView" name="paramTree"/>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/plotTab.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/plotTab.ui
new file mode 100755
index 0000000000000000000000000000000000000000..c5e28c026f28f3474883d124551e971a1746ef72
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/tabs/plotTab.ui
@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>765</width>
+ <height>384</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Plot</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QComboBox" name="dataSelector"/>
+ </item>
+ <item>
+ <layout class="QVBoxLayout" name="plotLayout"/>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.py
new file mode 100755
index 0000000000000000000000000000000000000000..79c8f0103f7d4254aae81223f15c70c5bded9606
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+A detachable toolbox for showing console printouts from the Crazyflie
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ConsoleToolbox']
+
+import sys, time
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal
+
+console_class = uic.loadUiType(sys.path[0] + "/cfclient/ui/toolboxes/consoleToolbox.ui")[0]
+
+class ConsoleToolbox(QtGui.QWidget, console_class):
+ """Console toolbox for showing printouts from the Crazyflie"""
+ update = pyqtSignal(str)
+
+ def __init__(self, helper, *args):
+ super(ConsoleToolbox, self).__init__(*args)
+ self.setupUi(self)
+
+ self.update.connect(self.console.insertPlainText)
+
+ self.helper = helper
+
+ def getName(self):
+ return 'Console'
+
+ def enable(self):
+ self.helper.cf.console.receivedChar.add_callback(self.update.emit)
+
+ def disable(self):
+ self.helper.cf.console.receivedChar.remove_callback(self.update.emit)
+
+ def preferedDockArea(self):
+ return Qt.BottomDockWidgetArea
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..0635198045bcde4805aa3c02761f9db148a5ce56
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/ConsoleToolbox.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.py
new file mode 100755
index 0000000000000000000000000000000000000000..37476512ef262c7ecc3fa5f5dd223996f3225fb6
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Toolbox for showing packets that is sent via the communication link when debugging.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['CrtpSharkBoolbox']
+
+import sys, time
+import os
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+from time import time
+
+param_tab_class = uic.loadUiType(sys.path[0] +
+ "/cfclient/ui/toolboxes/crtpSharkToolbox.ui")[0]
+
+class CrtpSharkToolbox(QtGui.QWidget, param_tab_class):
+ """Show packets that is sent vie the communication link"""
+ nameModified = pyqtSignal()
+ _incoming_packet_signal = pyqtSignal(object)
+ _outgoing_packet_signal = pyqtSignal(object)
+
+ def __init__(self, helper, *args):
+ super(CrtpSharkToolbox, self).__init__(*args)
+ self.setupUi(self)
+
+ self.helper = helper
+
+ #Init the tree widget
+ self.logTree.setHeaderLabels(['ms', 'Direction', 'Port/Chan', 'Data'])
+
+ #Connect GUI signals
+ self.clearButton.clicked.connect(self.clearLog)
+ self.saveButton.clicked.connect(self._save_data)
+
+ self._incoming_packet_signal.connect(lambda p: self._packet("IN", p))
+ self._outgoing_packet_signal.connect(lambda p: self._packet("OUT", p))
+ self._ms_offset = int(round(time() * 1000))
+
+ self._data = []
+
+ def _packet(self, dir, pk):
+ if self.masterCheck.isChecked():
+ line = QtGui.QTreeWidgetItem()
+
+ ms_diff = int(round(time()*1000))-self._ms_offset
+ line.setData(0, Qt.DisplayRole, "%d" % ms_diff)
+ line.setData(1, Qt.DisplayRole, "%s" % dir)
+ line.setData(2, Qt.DisplayRole, "%d/%d" % (pk.port, pk.channel))
+ line.setData(3, Qt.DisplayRole, pk.datal.__str__())
+
+ s = "%d, %s, %d/%d, %s" % (ms_diff, dir, pk.port, pk.channel,
+ pk.datal.__str__())
+ self._data.append(s)
+
+ self.logTree.addTopLevelItem(line)
+ self.logTree.scrollToItem(line)
+
+ @pyqtSlot()
+ def clearLog(self):
+ self.logTree.clear()
+ self._data = []
+
+ def getName(self):
+ return 'Crtp sniffer'
+
+ def getTabName(self):
+ return 'Crtp sniffer'
+
+ def enable(self):
+ self.helper.cf.packet_received.add_callback(
+ self._incoming_packet_signal.emit)
+ self.helper.cf.packet_sent.add_callback(
+ self._outgoing_packet_signal.emit)
+
+ def disable(self):
+ self.helper.cf.packet_received.remove_callback(
+ self._incoming_packet_signal.emit)
+ self.helper.cf.packet_sent.remove_callback(
+ self._outgoing_packet_signal.emit)
+
+ def preferedDockArea(self):
+ return Qt.RightDockWidgetArea
+
+ def _save_data(self):
+ dir = os.path.join(sys.path[1], "logdata")
+ fname = os.path.join(dir, "shark_data.csv")
+ if not os.path.exists(dir):
+ os.makedirs(dir)
+ f = open(fname, 'w')
+ for s in self._data:
+ f.write("%s\n" % s)
+ f.close()
+
+
+
+
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..15e2f34b5bcbf4cd9cc2eb10b5e248280ac71b27
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/CrtpSharkToolbox.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.py
new file mode 100755
index 0000000000000000000000000000000000000000..402b9cbb9a1ffc9de3d64caa96768842a0afa742
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Toolbox used to interact with the DebugDriver using a designated port. It's
+intended to be used for debugging.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['DebugDriverToolbox']
+
+import time
+import sys
+
+import struct
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
+
+debugdriver_tab_class = uic.loadUiType(
+ sys.path[0] +
+ "/cfclient/ui/toolboxes/debugDriverToolbox.ui")[0]
+
+
+class DebugDriverToolbox(QtGui.QWidget, debugdriver_tab_class):
+ """Used to interact with the DebugDriver toolbox"""
+ connectionDoneSignal = pyqtSignal(str)
+ disconnectedSignal = pyqtSignal(str)
+
+ def __init__(self, helper, *args):
+ super(DebugDriverToolbox, self).__init__(*args)
+ self.setupUi(self)
+
+ self.helper = helper
+
+ # Connected / disconnected signals
+ self.helper.cf.connected.add_callback(
+ self.connectionDoneSignal.emit)
+ self.connectionDoneSignal.connect(self.connectionDone)
+ self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
+ self.disconnectedSignal.connect(self.disconnected)
+
+ self.linkQuality.valueChanged.connect(self.linkQualityChanged)
+ self.forceDisconnect.pressed.connect(self.forceDisconnecPressed)
+
+ def forceDisconnecPressed(self):
+ if (self.helper.cf.link != None):
+ p = CRTPPacket()
+ p.set_header(CRTPPort.DEBUGDRIVER, 0)
+ p.data = struct.pack('<B', 1) # Force disconnect
+ self.helper.cf.send_packet(p)
+
+ def linkQualityChanged(self, value):
+ if (self.helper.cf.link != None):
+ p = CRTPPacket()
+ p.set_header(CRTPPort.DEBUGDRIVER, 0)
+ p.data = struct.pack('<BB', 0, value) # Set link quality
+ self.helper.cf.send_packet(p)
+
+ def disconnected(self, linkURI):
+ if ("debug" in linkURI):
+ self.linkQuality.setEnabled(False)
+ self.forceDisconnect.setEnabled(False)
+
+ def connectionDone(self, linkURI):
+ if ("debug" in linkURI):
+ self.linkQuality.setEnabled(True)
+ self.forceDisconnect.setEnabled(True)
+
+ def getName(self):
+ return 'Debug driver'
+
+ def getTabName(self):
+ return 'Debug driver'
+
+ def enable(self):
+ return
+
+ def disable(self):
+ return
+
+ def preferedDockArea(self):
+ return Qt.RightDockWidgetArea
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..8973d533d1112b1d954e0e5d7bcc9435755a2b54
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/DebugDriverToolbox.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..0ab24d4112add07602996b86bcc06c2271bc29c7
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+List all the available toolboxes so they can be used by the UI.
+
+Dropping a new .py file into this directory will automatically list and load
+it into the UI when it is started.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = []
+
+import os
+import glob
+import logging
+
+logger = logging.getLogger(__name__)
+
+foundToolboxes = [os.path.splitext(os.path.basename(f))[0] for f in
+ glob.glob(os.path.dirname(__file__) +
+ "/[A-Za-z]*Toolbox.py")]
+if len(foundToolboxes) == 0:
+ foundToolboxes = [os.path.splitext(os.path.basename(f))[0] for f in
+ glob.glob(os.path.dirname(__file__) +
+ "/[A-Za-z]*Toolbox.pyc")]
+
+
+logger.debug("Found toolboxes: %s", foundToolboxes)
+
+toolboxes = []
+
+for tb in foundToolboxes:
+ tbModule = __import__(tb, globals(), locals(), [tb], -1)
+ toolboxes.append(getattr(tbModule, tb))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..a2c0023e92b9b4013d7f43721d6c8aa271234f13
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/consoleToolbox.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/consoleToolbox.ui
new file mode 100755
index 0000000000000000000000000000000000000000..8b349b6889f171741fa41001eb596404acdedb9d
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/consoleToolbox.ui
@@ -0,0 +1,62 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>693</width>
+ <height>463</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QTextEdit" name="console">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="contextMenuPolicy">
+ <enum>Qt::DefaultContextMenu</enum>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="clearButton">
+ <property name="text">
+ <string>Clear</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections>
+ <connection>
+ <sender>clearButton</sender>
+ <signal>clicked()</signal>
+ <receiver>console</receiver>
+ <slot>clear()</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>346</x>
+ <y>437</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>346</x>
+ <y>212</y>
+ </hint>
+ </hints>
+ </connection>
+ </connections>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/crtpSharkToolbox.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/crtpSharkToolbox.ui
new file mode 100755
index 0000000000000000000000000000000000000000..e2f017065d1af7e265e6330c3a33d05408e16090
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/crtpSharkToolbox.ui
@@ -0,0 +1,92 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1187</width>
+ <height>751</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QTreeWidget" name="logTree">
+ <property name="columnCount">
+ <number>4</number>
+ </property>
+ <attribute name="headerDefaultSectionSize">
+ <number>100</number>
+ </attribute>
+ <column>
+ <property name="text">
+ <string notr="true">1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>2</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>3</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string notr="true">4</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="clearButton">
+ <property name="text">
+ <string>Clear</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="saveButton">
+ <property name="text">
+ <string>Save</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <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="QCheckBox" name="masterCheck">
+ <property name="text">
+ <string>Enable</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="checkGrid"/>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/debugDriverToolbox.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/debugDriverToolbox.ui
new file mode 100755
index 0000000000000000000000000000000000000000..915e3c35969644a08a7c103aa38a64101d795bec
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/toolboxes/debugDriverToolbox.ui
@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>400</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>DebugDriver Controls</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="checkGrid">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="forceDisconnect">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Force disconnect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="linkQuality">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>0</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Set Link Quality</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..8de7dc2610a7882fcefd5e7e6af6893ccc5a6676
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Custom widgets used by the user interface.
+"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..8563898fea878cd9191ad2659176eee65bf93907
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.py
new file mode 100755
index 0000000000000000000000000000000000000000..39a975fe45216c703e44dc11d631ccc88df9880f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.py
@@ -0,0 +1,268 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Attitude indicator widget.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['AttitudeIndicator']
+
+import sys
+from PyQt4 import QtGui, QtCore
+
+
+class AttitudeIndicator(QtGui.QWidget):
+ """Widget for showing attitude"""
+ def __init__(self):
+ super(AttitudeIndicator, self).__init__()
+
+ self.roll = 0
+ self.pitch = 0
+ self.hover = False
+ self.hoverASL = 0.0
+ self.hoverTargetASL = 0.0
+
+ self.setMinimumSize(30, 30)
+ # self.setMaximumSize(240,240)
+
+ def setRoll(self, roll):
+ self.roll = roll
+ self.repaint()
+
+ def setPitch(self, pitch):
+ self.pitch = pitch
+ self.repaint()
+
+ def setHover(self, target):
+ self.hoverTargetASL = target
+ self.hover = target>0
+ self.repaint()
+
+ def setBaro(self, asl):
+ self.hoverASL = asl;
+ self.repaint()
+
+ def setRollPitch(self, roll, pitch):
+ self.roll = roll
+ self.pitch = pitch
+ self.repaint()
+
+ def paintEvent(self, e):
+ qp = QtGui.QPainter()
+ qp.begin(self)
+ self.drawWidget(qp)
+ qp.end()
+
+ def drawWidget(self, qp):
+ size = self.size()
+ w = size.width()
+ h = size.height()
+
+ qp.translate(w / 2, h / 2)
+ qp.rotate(self.roll)
+ qp.translate(0, (self.pitch * h) / 50)
+ qp.translate(-w / 2, -h / 2)
+ qp.setRenderHint(qp.Antialiasing)
+
+ font = QtGui.QFont('Serif', 7, QtGui.QFont.Light)
+ qp.setFont(font)
+
+ # Draw the blue
+ qp.setPen(QtGui.QColor(0, 61, 144))
+ qp.setBrush(QtGui.QColor(0, 61, 144))
+ qp.drawRect(-w, h / 2, 3 * w, -3 * h)
+
+ # Draw the marron
+ qp.setPen(QtGui.QColor(59, 41, 39))
+ qp.setBrush(QtGui.QColor(59, 41, 39))
+ qp.drawRect(-w, h / 2, 3 * w, 3 * h)
+
+ pen = QtGui.QPen(QtGui.QColor(255, 255, 255), 1.5,
+ QtCore.Qt.SolidLine)
+ qp.setPen(pen)
+ qp.drawLine(-w, h / 2, 3 * w, h / 2)
+
+ # Drawing pitch lines
+ for ofset in [-180, 0, 180]:
+ for i in range(-900, 900, 25):
+ pos = (((i / 10.0) + 25 + ofset) * h / 50.0)
+ if i % 100 == 0:
+ length = 0.35 * w
+ if i != 0:
+ if ofset == 0:
+ qp.drawText((w / 2) + (length / 2) + (w * 0.06),
+ pos, "{}".format(-i / 10))
+ qp.drawText((w / 2) - (length / 2) - (w * 0.08),
+ pos, "{}".format(-i / 10))
+ else:
+ qp.drawText((w / 2) + (length / 2) + (w * 0.06),
+ pos, "{}".format(i / 10))
+ qp.drawText((w / 2) - (length / 2) - (w * 0.08),
+ pos, "{}".format(i / 10))
+ elif i % 50 == 0:
+ length = 0.2 * w
+ else:
+ length = 0.1 * w
+
+ qp.drawLine((w / 2) - (length / 2), pos,
+ (w / 2) + (length / 2), pos)
+
+ qp.setWorldMatrixEnabled(False)
+
+ pen = QtGui.QPen(QtGui.QColor(0, 0, 0), 2,
+ QtCore.Qt.SolidLine)
+ qp.setBrush(QtGui.QColor(0, 0, 0))
+ qp.setPen(pen)
+ qp.drawLine(0, h / 2, w, h / 2)
+
+
+
+ # Draw Hover vs Target
+
+ qp.setWorldMatrixEnabled(False)
+
+ pen = QtGui.QPen(QtGui.QColor(255, 255, 255), 2,
+ QtCore.Qt.SolidLine)
+ qp.setBrush(QtGui.QColor(255, 255, 255))
+ qp.setPen(pen)
+ fh = max(7,h/50)
+ font = QtGui.QFont('Sans', fh, QtGui.QFont.Light)
+ qp.setFont(font)
+ qp.resetTransform()
+
+
+
+
+ qp.translate(0,h/2)
+ if not self.hover:
+ qp.drawText(w-fh*10, fh/2, str(round(self.hoverASL,2))) # asl
+
+
+ if self.hover:
+ qp.drawText(w-fh*10, fh/2, str(round(self.hoverTargetASL,2))) # target asl (center)
+ diff = round(self.hoverASL-self.hoverTargetASL,2)
+ pos_y = -h/6*diff
+
+ # cap to +- 2.8m
+ if diff<-2.8:
+ pos_y = -h/6*-2.8
+ elif diff>2.8:
+ pos_y= -h/6*2.8
+ else:
+ pos_y = -h/6*diff
+ qp.drawText(w-fh*3.8, pos_y+fh/2, str(diff)) # difference from target (moves up and down +- 2.8m)
+ qp.drawLine(w-fh*4.5,0,w-fh*4.5,pos_y) # vertical line
+ qp.drawLine(w-fh*4.7,0,w-fh*4.5,0) # left horizontal line
+ qp.drawLine(w-fh*4.2,pos_y,w-fh*4.5,pos_y) #right horizontal line
+
+
+
+
+
+if __name__ == "__main__":
+ class Example(QtGui.QWidget):
+
+ def __init__(self):
+ super(Example, self).__init__()
+
+ self.initUI()
+
+ def updatePitch(self, pitch):
+ self.wid.setPitch(pitch - 90)
+
+ def updateRoll(self, roll):
+ self.wid.setRoll((roll / 10.0) - 180.0)
+
+ def updateTarget(self, target):
+ self.wid.setHover(500+target/10.)
+ def updateBaro(self, asl):
+ self.wid.setBaro(500+asl/10.)
+
+
+ def initUI(self):
+
+ vbox = QtGui.QVBoxLayout()
+
+ sld = QtGui.QSlider(QtCore.Qt.Horizontal, self)
+ sld.setFocusPolicy(QtCore.Qt.NoFocus)
+ sld.setRange(0, 3600)
+ sld.setValue(1800)
+ vbox.addWidget(sld)
+
+
+ self.wid = AttitudeIndicator()
+
+ sld.valueChanged[int].connect(self.updateRoll)
+ vbox.addWidget(self.wid)
+
+ hbox = QtGui.QHBoxLayout()
+ hbox.addLayout(vbox)
+
+ sldPitch = QtGui.QSlider(QtCore.Qt.Vertical, self)
+ sldPitch.setFocusPolicy(QtCore.Qt.NoFocus)
+ sldPitch.setRange(0, 180)
+ sldPitch.setValue(90)
+ sldPitch.valueChanged[int].connect(self.updatePitch)
+ hbox.addWidget(sldPitch)
+
+ sldASL = QtGui.QSlider(QtCore.Qt.Vertical, self)
+ sldASL.setFocusPolicy(QtCore.Qt.NoFocus)
+ sldASL.setRange(-200, 200)
+ sldASL.setValue(0)
+ sldASL.valueChanged[int].connect(self.updateBaro)
+
+ sldT = QtGui.QSlider(QtCore.Qt.Vertical, self)
+ sldT.setFocusPolicy(QtCore.Qt.NoFocus)
+ sldT.setRange(-200, 200)
+ sldT.setValue(0)
+ sldT.valueChanged[int].connect(self.updateTarget)
+
+ hbox.addWidget(sldT)
+ hbox.addWidget(sldASL)
+
+
+ self.setLayout(hbox)
+
+ self.setGeometry(50, 50, 510, 510)
+ self.setWindowTitle('Attitude Indicator')
+ self.show()
+
+ def changeValue(self, value):
+
+ self.c.updateBW.emit(value)
+ self.wid.repaint()
+
+ def main():
+
+ app = QtGui.QApplication(sys.argv)
+ ex = Example()
+ sys.exit(app.exec_())
+
+
+ if __name__ == '__main__':
+ main()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..7214cb8692aab93825ea2ad0850495e247bce2a7
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/ai.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.py
new file mode 100755
index 0000000000000000000000000000000000000000..82eab2e950306854b0d11664d2c39b18cd9013d6
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+This class provides a spin box with hexadecimal numbers and arbitrarily length (i.e. not limited by 32 bit).
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['HexSpinBox']
+
+from PyQt4 import QtGui, QtCore
+from PyQt4.QtGui import QAbstractSpinBox
+
+class HexSpinBox(QAbstractSpinBox):
+ def __init__(self, *args):
+ QAbstractSpinBox.__init__(self, *args)
+ regexp = QtCore.QRegExp('^0x[0-9A-Fa-f]{1,10}$')
+ self.validator = QtGui.QRegExpValidator(regexp)
+ self._value = 0
+
+ def validate(self, text, pos):
+ return self.validator.validate(text, pos)
+
+ def textFromValue(self, value):
+ return "0x%X" % value
+
+ def valueFromText(self, text):
+ return int(str(text), 0)
+
+ def setValue(self, value):
+ self._value = value
+ self.lineEdit().setText(self.textFromValue(value))
+
+ def value(self):
+ self._value = self.valueFromText(self.lineEdit().text())
+ return self._value
+
+ def stepBy(self, steps):
+ self.setValue(self._value + steps)
+
+ def stepEnabled(self):
+ return QAbstractSpinBox.StepUpEnabled | QAbstractSpinBox.StepDownEnabled
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..fe4095fa3bfd81eede39080a259b2554c64d7b83
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/hexspinbox.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotter.ui b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotter.ui
new file mode 100755
index 0000000000000000000000000000000000000000..bfd23dd1165c98bfd03bce344150611308c366fe
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotter.ui
@@ -0,0 +1,288 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>926</width>
+ <height>499</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Plot</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <layout class="QVBoxLayout" name="mainLayout">
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <layout class="QVBoxLayout" name="plotLayout"/>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="lowerLayout">
+ <item>
+ <layout class="QGridLayout" name="options">
+ <item row="3" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="0">
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="0" column="1">
+ <widget class="QLineEdit" name="_range_x_min">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>0</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QRadioButton" name="_enable_samples_x">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Samples</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="autoExclusive">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSpinBox" name="_nbr_of_samples_x">
+ <property name="suffix">
+ <string/>
+ </property>
+ <property name="maximum">
+ <number>2000</number>
+ </property>
+ <property name="singleStep">
+ <number>100</number>
+ </property>
+ <property name="value">
+ <number>500</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLineEdit" name="_range_x_max">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>1000</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QRadioButton" name="_enable_range_x">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Range</string>
+ </property>
+ <property name="autoExclusive">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QRadioButton" name="_enable_manual_x">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Manual</string>
+ </property>
+ <property name="autoExclusive">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QRadioButton" name="_enable_seconds_x">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Seconds</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QSpinBox" name="_nbr_of_seconds_x">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="suffix">
+ <string/>
+ </property>
+ <property name="maximum">
+ <number>59</number>
+ </property>
+ <property name="value">
+ <number>1</number>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="2" column="2">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QRadioButton" name="_enable_range_y">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Range</string>
+ </property>
+ <property name="autoExclusive">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QRadioButton" name="_enable_auto_y">
+ <property name="text">
+ <string>Auto</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="autoExclusive">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <spacer name="horizontalSpacer_2">
+ <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 row="2" column="1">
+ <widget class="QDoubleSpinBox" name="_range_y_min">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimum">
+ <double>-65000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>-1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QDoubleSpinBox" name="_range_y_max">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="minimum">
+ <double>-65000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65000.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>1.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="1">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="0">
+ <spacer name="horizontalSpacer">
+ <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 row="0" column="1">
+ <widget class="QCheckBox" name="_auto_redraw">
+ <property name="text">
+ <string>Auto update graph</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>X Axis</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Y Axis</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.py
new file mode 100755
index 0000000000000000000000000000000000000000..459be6f6712c6a9467948d96695cb25db7e27e77
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.py
@@ -0,0 +1,291 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Container for the simple plot with functionality for data legend, saving data
+and manipulating the plot.
+
+For more advanced plotting save the data and use an external application.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PlotWidget']
+
+from PyQt4 import QtCore, QtGui, uic
+from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, QLine, QPoint, QPointF, QSize, QRectF
+
+from time import time
+import math
+
+import logging
+
+logger = logging.getLogger(__name__)
+
+import sys
+
+from PyQt4 import Qt, QtCore, QtGui, uic
+from PyQt4.QtGui import QButtonGroup
+from PyQt4.QtCore import *
+from PyQt4.QtGui import *
+from PyQt4.Qt import *
+from time import time
+
+(plot_widget_class,
+connect_widget_base_class) = (uic.loadUiType(
+ sys.path[0] + '/cfclient/ui/widgets/plotter.ui'))
+
+# Try the imports for PyQtGraph to see if it is installed
+try:
+ import pyqtgraph as pg
+ from pyqtgraph import ViewBox
+ from pyqtgraph.Qt import QtCore, QtGui
+ import pyqtgraph.console
+ import numpy as np
+
+ _pyqtgraph_found = True
+except Exception:
+ import traceback
+ logger.warning("PyQtGraph (or dependency) failed to import:\n%s",
+ traceback.format_exc())
+ _pyqtgraph_found = False
+
+# This is required to force py2exe to pull in the correct dependencies on
+# Windows. But for Linux this is not required and might not be installed with
+# the PyQtGraph package.
+try:
+ from scipy.stats import futil
+ from scipy.sparse.csgraph import _validation
+ from scipy.special import _ufuncs_cxx
+except Exception:
+ pass
+
+class PlotItemWrapper:
+ """Wrapper for PlotDataItem to handle what data is shown"""
+ def __init__(self, curve):
+ """Initialize"""
+ self.data = []
+ self.ts = []
+ self.curve = curve
+
+ def add_point(self, p, ts):
+ """
+ Add a point to the curve.
+
+ p - point
+ ts - timestamp in ms
+ """
+ self.data.append(p)
+ self.ts.append(ts)
+
+ def show_data(self, start, stop):
+ """Set what data should be shown from the curve. This is done to keep performance when many
+ points have been added."""
+ limit = min(stop, len(self.data))
+ self.curve.setData(y=self.data[start:limit], x=self.ts[start:limit])
+ return [self.ts[start], self.ts[limit-1]]
+
+class PlotWidget(QtGui.QWidget, plot_widget_class):
+ """Wrapper widget for PyQtGraph adding some extra buttons"""
+
+ def __init__(self, parent=None, fps=100, title="", *args):
+ super(PlotWidget, self).__init__(*args)
+ self.setupUi(self)
+
+ # Limit the plot update to 10Hz
+ self._ts = time()
+ self._delay = 0.1
+
+ # Check if we could import PyQtGraph, if not then stop here
+ if not _pyqtgraph_found:
+ self.can_enable = False
+ return
+ else:
+ self.can_enable = True
+
+ self._items = {}
+ self._last_item = 0
+
+ self.setSizePolicy(QtGui.QSizePolicy(
+ QtGui.QSizePolicy.MinimumExpanding,
+ QtGui.QSizePolicy.MinimumExpanding))
+
+ self.setMinimumSize(self.minimumSizeHint())
+ self.parent = parent
+
+ pg.setConfigOption('background', 'w')
+ pg.setConfigOption('foreground', 'k')
+ self._plot_widget = pg.PlotWidget()
+ self._plot_widget.hideButtons()
+ self._plot_widget.setLabel('bottom', "Time", "ms")
+ self._plot_widget.addLegend()
+ self._plot_widget.getViewBox().disableAutoRange(ViewBox.XAxis)
+ self._plot_widget.getViewBox().sigRangeChangedManually.connect(self._manual_range_change)
+ self._plot_widget.getViewBox().setMouseEnabled(x=False, y=True)
+ self._plot_widget.getViewBox().setMouseMode(ViewBox.PanMode)
+
+ self.plotLayout.addWidget(self._plot_widget)
+
+ #self.saveToFile.clicked.connect(self.saveToFileSignal)
+ self._x_min = 0
+ self._x_max = 500
+ self._enable_auto_y.setChecked(True)
+ self._enable_samples_x.setChecked(True)
+ self._last_ts = None
+ self._dtime = None
+
+ self._x_range = (float(self._range_x_min.text()), float(self._range_x_max.text()))
+ self._nbr_samples = int(self._nbr_of_samples_x.text())
+
+ self._nbr_of_samples_x.valueChanged.connect(self._nbr_samples_changed)
+ self._range_y_min.valueChanged.connect(self._y_range_changed)
+ self._range_y_max.valueChanged.connect(self._y_range_changed)
+
+ self._y_btn_group = QButtonGroup()
+ self._y_btn_group.addButton(self._enable_auto_y)
+ self._y_btn_group.addButton(self._enable_range_y)
+ self._y_btn_group.setExclusive(True)
+ self._y_btn_group.buttonClicked.connect(self._y_mode_change)
+
+ self._x_btn_group = QButtonGroup()
+ self._x_btn_group.addButton(self._enable_range_x)
+ self._x_btn_group.addButton(self._enable_samples_x)
+ self._x_btn_group.addButton(self._enable_seconds_x)
+ self._x_btn_group.addButton(self._enable_manual_x)
+ self._x_btn_group.setExclusive(True)
+ self._x_btn_group.buttonClicked.connect(self._x_mode_change)
+
+ self._draw_graph = True
+ self._auto_redraw.stateChanged.connect(self._auto_redraw_change)
+
+ def _auto_redraw_change(self, state):
+ """Callback from the auto redraw checkbox"""
+ if state == 0:
+ self._draw_graph = False
+ else:
+ self._draw_graph = True
+
+ def _x_mode_change(self, box):
+ """Callback when user changes the X-axis mode"""
+ if box == self._enable_range_x:
+ logger.info("Enable range x")
+ self._x_range = (float(self._range_x_min.text()), float(self._range_x_max.text()))
+ else:
+ self._range_x_min.setEnabled(False)
+ self._range_x_max.setEnabled(False)
+
+ def _y_mode_change(self, box):
+ """Callback when user changes the Y-axis mode"""
+ if box == self._enable_range_y:
+ self._range_y_min.setEnabled(True)
+ self._range_y_max.setEnabled(True)
+ y_range = (float(self._range_y_min.value()), float(self._range_y_max.value()))
+ self._plot_widget.getViewBox().setRange(yRange=y_range)
+ else:
+ self._range_y_min.setEnabled(False)
+ self._range_y_max.setEnabled(False)
+
+ if box == self._enable_auto_y:
+ self._plot_widget.getViewBox().enableAutoRange(ViewBox.YAxis)
+
+ def _manual_range_change(self, obj):
+ """Callback from pyqtplot when users changes the range of the plot using the mouse"""
+ [[x_min,x_max],[y_min,y_max]] = self._plot_widget.getViewBox().viewRange()
+ self._range_y_min.setValue(y_min)
+ self._range_y_max.setValue(y_max)
+ self._range_y_min.setEnabled(True)
+ self._range_y_max.setEnabled(True)
+ self._enable_range_y.setChecked(True)
+
+ def _y_range_changed(self, val):
+ """Callback when user changes Y range manually"""
+ _y_range = (float(self._range_y_min.value()), float(self._range_y_max.value()))
+ self._plot_widget.getViewBox().setRange(yRange=_y_range, padding=0)
+
+ def _nbr_samples_changed(self, val):
+ """Callback when user changes the number of samples to be shown"""
+ self._nbr_samples = val
+
+ def set_title(self, title):
+ """
+ Set the title of the plot.
+
+ title - the new title
+ """
+ self._plot_widget.setTitle(title)
+
+ def add_curve(self, title, pen='r'):
+ """
+ Add a new curve to the plot.
+
+ title - the name of the data
+ pen - color of curve (using r for red and so on..)
+ """
+ self._items[title] = PlotItemWrapper(self._plot_widget.plot(name=title, pen=pen))
+
+ def add_data(self, data, ts):
+ """
+ Add new data to the plot.
+
+ data - dictionary sent from logging layer containing variable/value pairs
+ ts - timestamp of the data in ms
+ """
+ if not self._last_ts:
+ self._last_ts = ts
+ elif not self._last_ts:
+ self._dtime = ts - self._last_ts
+ self._last_ts = ts
+
+ x_min_limit = 0
+ x_max_limit = 0
+ # We are adding new datasets, calculate what we should show.
+ if self._enable_samples_x.isChecked():
+ x_min_limit = max(0, self._last_item-self._nbr_samples)
+ x_max_limit = max(self._last_item, self._nbr_samples)
+
+ for name in self._items:
+ self._items[name].add_point(data[name], ts)
+ if self._draw_graph and time() > self._ts + self._delay:
+ [self._x_min, self._x_max] = self._items[name].show_data(x_min_limit, x_max_limit)
+ if time() > self._ts + self._delay:
+ self._ts = time()
+ if self._enable_samples_x.isChecked() and self._dtime and self._last_item < self._nbr_samples:
+ self._x_max = self._x_min + self._nbr_samples * self._dtime
+
+ self._last_item = self._last_item + 1
+ self._plot_widget.getViewBox().setRange(xRange=(self._x_min, self._x_max))
+
+ def removeAllDatasets(self):
+ """Reset the plot by removing all the datasets"""
+ for item in self._items:
+ self._plot_widget.removeItem(self._items[item])
+ self._plot_widget.plotItem.legend.items = []
+ self._items = {}
+ self._last_item = 0
+ self._last_ts = None
+ self._dtime = None
+ self._plot_widget.clear()
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..a6cc6be362d25fd29aa4af50696e22e375fe8247
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/ui/widgets/plotwidget.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..8a252d5b6b0c825875f539ccfd6b8da37e62d992
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Various utilities used by the user interface.
+"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..5859123c9c6fa6c4519f23a61276a26911af398d
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.py
new file mode 100755
index 0000000000000000000000000000000000000000..e7e09816eec25180e81b1990bb68ee100593d224
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.py
@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Gives access for reading and writing application configuration parameters
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Config']
+
+import sys
+import json
+import logging
+from .singleton import Singleton
+
+logger = logging.getLogger(__name__)
+
+class Config():
+ """ Singleton class for accessing application configuration """
+ __metaclass__ = Singleton
+ def __init__(self):
+ """ Initializes the singleton and reads the config files """
+ self._dist_config = sys.path[0] + "/cfclient/configs/config.json"
+ self._config = sys.path[1] + "/config.json"
+
+ [self._readonly, self._data] = self._read_distfile()
+
+ user_config = self._read_config()
+ if (user_config):
+ self._data.update(user_config)
+
+ def _read_distfile(self):
+ """ Read the distribution config file containing the defaults """
+ f = open(self._dist_config, 'r')
+ data = json.load(f)
+ f.close()
+ logger.info("Dist config read from %s" % self._dist_config)
+
+ return [data["read-only"], data["writable"]]
+
+ def set(self, key, value):
+ """ Set the value of a config parameter """
+ try:
+ self._data[key] = value
+ except KeyError:
+ raise KeyError("Could not set the parameter [%s]" % key)
+
+ def get(self, key):
+ """ Get the value of a config parameter """
+ value = None
+ if (key in self._data):
+ value = self._data[key]
+ elif (key in self._readonly):
+ value = self._readonly[key]
+ else:
+ raise KeyError("Could not get the parameter [%s]" % key)
+
+ if (isinstance(value, unicode)):
+ value = str(value)
+
+ return value
+
+ def save_file(self):
+ """ Save the user config to file """
+ json_data = open(self._config, 'w')
+ json_data.write(json.dumps(self._data, indent=2))
+ json_data.close()
+ logger.info("Config file saved to [%s]" % self._config)
+
+ def _read_config(self):
+ """ Read the user config from file """
+ try:
+ json_data = open(self._config)
+ data = json.load(json_data)
+ json_data.close()
+ logger.info("Config file read from [%s]" % self._config)
+ except Exception:
+ return None
+
+ return data
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..484e51ab008a24f203c54cb006c29fea6489473f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.py
new file mode 100755
index 0000000000000000000000000000000000000000..8da823aa74ebecea8f9ea51ec470b7ffdd512cc7
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+# Copyright (C) 2013 Allyn Bauer
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Manager for loading/accesing input device mappings.
+"""
+
+__author__ = 'Bitcraze AB/Allyn Bauer'
+__all__ = ['ConfigManager']
+
+import sys
+import json
+import logging
+import glob
+import os
+import copy
+
+from .singleton import Singleton
+from cflib.utils.callbacks import Caller
+
+logger = logging.getLogger(__name__)
+
+
+class ConfigManager():
+ """ Singleton class for managing input processing """
+ conf_needs_reload = Caller()
+ configs_dir = sys.path[1] + "/input"
+
+ __metaclass__ = Singleton
+
+ def __init__(self):
+ """Initialize and create empty config list"""
+ self._list_of_configs = []
+
+ def get_config(self, config_name):
+ """Get the button and axis mappings for an input device."""
+ try:
+ idx = self._list_of_configs.index(config_name)
+ return self._input_config[idx]
+ except:
+ return None
+
+ def get_settings(self, config_name):
+ """Get the settings for an input device."""
+ try:
+ idx = self._list_of_configs.index(config_name)
+ return self._input_settings[idx]
+ except:
+ return None
+
+ def get_list_of_configs(self):
+ """Reload the configurations from file"""
+ try:
+ configs = [os.path.basename(f) for f in
+ glob.glob(self.configs_dir + "/[A-Za-z]*.json")]
+ self._input_config = []
+ self._input_settings = []
+ self._list_of_configs = []
+ for conf in configs:
+ logger.debug("Parsing [%s]", conf)
+ json_data = open(self.configs_dir + "/%s" % conf)
+ data = json.load(json_data)
+ new_input_device = {}
+ new_input_settings = {"updateperiod":10, "springythrottle":True}
+ for s in data["inputconfig"]["inputdevice"]:
+ if s == "axis":
+ for a in data["inputconfig"]["inputdevice"]["axis"]:
+ axis = {}
+ axis["scale"] = a["scale"]
+ axis["offset"] = a["offset"] if "offset" in a else 0.0
+ axis["type"] = a["type"]
+ axis["key"] = a["key"]
+ axis["name"] = a["name"]
+ try:
+ ids = a["ids"]
+ except:
+ ids = [a["id"]]
+ for id in ids:
+ locaxis = copy.deepcopy(axis)
+ if "ids" in a:
+ if id == a["ids"][0]:
+ locaxis["scale"] = locaxis["scale"] * -1
+ locaxis["id"] = id
+ # 'type'-'id' defines unique index for axis
+ index = "%s-%d" % (a["type"], id)
+ new_input_device[index] = locaxis
+ else:
+ new_input_settings[s] = data["inputconfig"]["inputdevice"][s]
+ self._input_config.append(new_input_device)
+ self._input_settings.append(new_input_settings)
+ json_data.close()
+ self._list_of_configs.append(conf[:-5])
+ except Exception as e:
+ logger.warning("Exception while parsing inputconfig file: %s ", e)
+ return self._list_of_configs
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..51f6358a17701d6345bff2f8a43213c3bd918ffc
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/config_manager.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/guiconfig.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/guiconfig.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..c08a5488a3f3fbab088fa8501cfa241ea285020f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/guiconfig.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..3e22bf8fd4eddad80cf08030c3f123c337ac4114
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..34d3c4d1a239e65de756edf2a5691831c41f3ab2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.py
@@ -0,0 +1,412 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Module to read input devices and send controls to the Crazyflie.
+
+This module reads input from joysticks or other input devices and sends control
+set-points to the Crazyflie. It can be configured in the UI.
+
+Various drivers can be used to read input device data. Currently is uses the
+PySDL2 driver, but in the future native support will be provided for Linux and
+Windows drivers.
+
+The input device's axes and buttons are mapped to software inputs using a
+configuration file.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['JoystickReader']
+
+import sys
+import os
+import re
+import glob
+import traceback
+import logging
+import shutil
+
+import inputreaders as readers
+import inputinterfaces as interfaces
+
+logger = logging.getLogger(__name__)
+
+from cfclient.utils.config import Config
+from cfclient.utils.config_manager import ConfigManager
+
+from cfclient.utils.periodictimer import PeriodicTimer
+from cflib.utils.callbacks import Caller
+import mux
+from .mux import InputMux
+from .mux.nomux import NoMux
+from .mux.takeovermux import TakeOverMux
+from .mux.takeoverselectivemux import TakeOverSelectiveMux
+
+MAX_THRUST = 65000
+
+class JoystickReader(object):
+ """
+ Thread that will read input from devices/joysticks and send control-set
+ points to the Crazyflie
+ """
+ inputConfig = []
+
+ def __init__(self, do_device_discovery=True):
+ self._input_device = None
+
+ self.min_thrust = 0
+ self.max_thrust = 0
+ self._thrust_slew_rate = 0
+ self.thrust_slew_enabled = False
+ self.thrust_slew_limit = 0
+ self.has_pressure_sensor = False
+
+ self.max_rp_angle = 0
+ self.max_yaw_rate = 0
+
+ self._old_thrust = 0
+ self._old_raw_thrust = 0
+ self._old_alt_hold = False
+ self.springy_throttle = True
+
+ self.trim_roll = Config().get("trim_roll")
+ self.trim_pitch = Config().get("trim_pitch")
+
+ self._input_map = None
+
+ self._mux = [NoMux(self), TakeOverSelectiveMux(self), TakeOverMux(self)]
+ # Set NoMux as default
+ self._selected_mux = self._mux[0]
+
+ if Config().get("flightmode") is "Normal":
+ self.max_yaw_rate = Config().get("normal_max_yaw")
+ self.max_rp_angle = Config().get("normal_max_rp")
+ # Values are stored at %, so use the functions to set the values
+ self.min_thrust = Config().get("normal_min_thrust")
+ self.max_thrust = Config().get("normal_max_thrust")
+ self.thrust_slew_limit = Config().get("normal_slew_limit")
+ self.thrust_slew_rate = Config().get("normal_slew_rate")
+ else:
+ self.max_yaw_rate = Config().get("max_yaw")
+ self.max_rp_angle = Config().get("max_rp")
+ # Values are stored at %, so use the functions to set the values
+ self.min_thrust = Config().get("min_thrust")
+ self.max_thrust = Config().get("max_thrust")
+ self.thrust_slew_limit = Config().get("slew_limit")
+ self.thrust_slew_rate = Config().get("slew_rate")
+
+ self._dev_blacklist = None
+ if len(Config().get("input_device_blacklist")) > 0:
+ self._dev_blacklist = re.compile(
+ Config().get("input_device_blacklist"))
+ logger.info("Using device blacklist [{}]".format(
+ Config().get("input_device_blacklist")))
+
+
+ self._available_devices = {}
+
+ # TODO: The polling interval should be set from config file
+ self._read_timer = PeriodicTimer(0.01, self.read_input)
+
+ if do_device_discovery:
+ self._discovery_timer = PeriodicTimer(1.0,
+ self._do_device_discovery)
+ self._discovery_timer.start()
+
+ # Check if user config exists, otherwise copy files
+ if not os.path.exists(ConfigManager().configs_dir):
+ logger.info("No user config found, copying dist files")
+ os.makedirs(ConfigManager().configs_dir)
+
+ for f in glob.glob(sys.path[0] +
+ "/cfclient/configs/input/[A-Za-z]*.json"):
+ dest = os.path.join(ConfigManager().
+ configs_dir, os.path.basename(f))
+ if not os.path.isfile(dest):
+ logger.debug("Copying %s", f)
+ shutil.copy2(f, ConfigManager().configs_dir)
+
+ ConfigManager().get_list_of_configs()
+
+ self.input_updated = Caller()
+ self.rp_trim_updated = Caller()
+ self.emergency_stop_updated = Caller()
+ self.device_discovery = Caller()
+ self.device_error = Caller()
+ self.althold_updated = Caller()
+ self.alt1_updated = Caller()
+ self.alt2_updated = Caller()
+
+ # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
+ self.limiting_updated = Caller()
+
+ def _get_device_from_name(self, device_name):
+ """Get the raw device from a name"""
+ for d in readers.devices():
+ if d.name == device_name:
+ return d
+ return None
+
+ def set_alt_hold_available(self, available):
+ """Set if altitude hold is available or not (depending on HW)"""
+ self.has_pressure_sensor = available
+
+ def enable_alt_hold(self, althold):
+ """Enable or disable altitude hold"""
+ self._old_alt_hold = althold
+
+ def _do_device_discovery(self):
+ devs = self.available_devices()
+
+ # This is done so that devs can easily get access
+ # to limits without creating lots of extra code
+ for d in devs:
+ d.input = self
+
+ if len(devs):
+ self.device_discovery.call(devs)
+ self._discovery_timer.stop()
+
+ def available_mux(self):
+ return self._mux
+
+ def set_mux(self, name=None, mux=None):
+ old_mux = self._selected_mux
+ if name:
+ for m in self._mux:
+ if m.name == name:
+ self._selected_mux = m
+ elif mux:
+ self._selected_mux = mux
+
+ old_mux.close()
+
+ logger.info("Selected MUX: {}".format(self._selected_mux.name))
+
+ def available_devices(self):
+ """List all available and approved input devices.
+ This function will filter available devices by using the
+ blacklist configuration and only return approved devices."""
+ devs = readers.devices()
+ devs += interfaces.devices()
+ approved_devs = []
+
+ for dev in devs:
+ if ((not self._dev_blacklist) or
+ (self._dev_blacklist and not
+ self._dev_blacklist.match(dev.name))):
+ dev.input = self
+ approved_devs.append(dev)
+
+ return approved_devs
+
+ def enableRawReading(self, device_name):
+ """
+ Enable raw reading of the input device with id deviceId. This is used
+ to get raw values for setting up of input devices. Values are read
+ without using a mapping.
+ """
+ if self._input_device:
+ self._input_device.close()
+ self._input_device = None
+
+ for d in readers.devices():
+ if d.name == device_name:
+ self._input_device = d
+
+ # Set the mapping to None to get raw values
+ self._input_device.input_map = None
+ self._input_device.open()
+
+ def get_saved_device_mapping(self, device_name):
+ """Return the saved mapping for a given device"""
+ config = None
+ device_config_mapping = Config().get("device_config_mapping")
+ if device_name in device_config_mapping.keys():
+ config = device_config_mapping[device_name]
+
+ logging.debug("For [{}] we recommend [{}]".format(device_name, config))
+ return config
+
+ def stop_raw_reading(self):
+ """Disable raw reading of input device."""
+ if self._input_device:
+ self._input_device.close()
+ self._input_device = None
+
+ def read_raw_values(self):
+ """ Read raw values from the input device."""
+ [axes, buttons, mapped_values] = self._input_device.read(include_raw=True)
+ dict_axes = {}
+ dict_buttons = {}
+
+ for i, a in enumerate(axes):
+ dict_axes[i] = a
+
+ for i, b in enumerate(buttons):
+ dict_buttons[i] = b
+
+ return [dict_axes, dict_buttons, mapped_values]
+
+ def set_raw_input_map(self, input_map):
+ """Set an input device map"""
+ # TODO: Will not work!
+ if self._input_device:
+ self._input_device.input_map = input_map
+
+ def set_input_map(self, device_name, input_map_name):
+ """Load and set an input device map with the given name"""
+ settings = ConfigManager().get_settings(input_map_name)
+ if settings:
+ self.springy_throttle = settings["springythrottle"]
+ self._input_map = ConfigManager().get_config(input_map_name)
+ self._get_device_from_name(device_name).input_map = self._input_map
+ self._get_device_from_name(device_name).input_map_name = input_map_name
+ Config().get("device_config_mapping")[device_name] = input_map_name
+
+ def start_input(self, device_name, role="Device", config_name=None):
+ """
+ Start reading input from the device with name device_name using config
+ config_name. Returns True if device supports mapping, otherwise False
+ """
+ try:
+ #device_id = self._available_devices[device_name]
+ # Check if we supplied a new map, if not use the preferred one
+ device = self._get_device_from_name(device_name)
+ self._selected_mux.add_device(device, role)
+ # Update the UI with the limiting for this device
+ self.limiting_updated.call(device.limit_rp,
+ device.limit_yaw,
+ device.limit_thrust)
+ self._read_timer.start()
+ return device.supports_mapping
+ except Exception:
+ self.device_error.call(
+ "Error while opening/initializing input device\n\n%s" %
+ (traceback.format_exc()))
+
+ if not self._input_device:
+ self.device_error.call(
+ "Could not find device {}".format(device_name))
+ return False
+
+ def resume_input(self):
+ self._selected_mux.resume()
+ self._read_timer.start()
+
+ def pause_input(self, device_name = None):
+ """Stop reading from the input device."""
+ self._read_timer.stop()
+ self._selected_mux.pause()
+
+ def _set_thrust_slew_rate(self, rate):
+ self._thrust_slew_rate = rate
+ if rate > 0:
+ self.thrust_slew_enabled = True
+ else:
+ self.thrust_slew_enabled = False
+
+ def _get_thrust_slew_rate(self):
+ return self._thrust_slew_rate
+
+ def read_input(self):
+ """Read input data from the selected device"""
+ try:
+ data = self._selected_mux.read()
+
+ if data:
+ if data.toggled.althold:
+ try:
+ self.althold_updated.call(str(data.althold))
+ except Exception as e:
+ logger.warning("Exception while doing callback from"
+ "input-device for althold: {}".format(e))
+
+ if data.toggled.estop:
+ try:
+ self.emergency_stop_updated.call(data.estop)
+ except Exception as e:
+ logger.warning("Exception while doing callback from"
+ "input-device for estop: {}".format(e))
+
+ if data.toggled.alt1:
+ try:
+ self.alt1_updated.call(data.alt1)
+ except Exception as e:
+ logger.warning("Exception while doing callback from"
+ "input-device for alt1: {}".format(e))
+ if data.toggled.alt2:
+ try:
+ self.alt2_updated.call(data.alt2)
+ except Exception as e:
+ logger.warning("Exception while doing callback from"
+ "input-device for alt2: {}".format(e))
+
+ # Update the user roll/pitch trim from device
+ if data.toggled.pitchNeg and data.pitchNeg:
+ self.trim_pitch -= 1
+ if data.toggled.pitchPos and data.pitchPos:
+ self.trim_pitch += 1
+ if data.toggled.rollNeg and data.rollNeg:
+ self.trim_roll -= 1
+ if data.toggled.rollPos and data.rollPos:
+ self.trim_roll += 1
+
+ if data.toggled.pitchNeg or data.toggled.pitchPos or \
+ data.toggled.rollNeg or data.toggled.rollPos:
+ self.rp_trim_updated.call(self.trim_roll, self.trim_pitch)
+
+ # Thrust might be <0 here, make sure it's not otherwise we'll
+ # get an error.
+ if data.thrust < 0:
+ data.thrust = 0
+ if data.thrust > 0xFFFF:
+ data.thrust = 0xFFFF
+
+ # If we are using alt hold the data is not in a percentage
+ if not data.althold:
+ data.thrust = JoystickReader.p2t(data.thrust)
+
+ self.input_updated.call(data.roll + self.trim_roll,
+ data.pitch + self.trim_pitch,
+ data.yaw, data.thrust)
+ else:
+ self.input_updated.call(0, 0, 0, 0)
+ except Exception:
+ logger.warning("Exception while reading inputdevice: %s",
+ traceback.format_exc())
+ self.device_error.call("Error reading from input device\n\n%s" %
+ traceback.format_exc())
+ self.input_updated.call(0, 0, 0, 0)
+ self._read_timer.stop()
+
+ @staticmethod
+ def p2t(percentage):
+ """Convert a percentage to raw thrust"""
+ return int(MAX_THRUST * (percentage / 100.0))
+
+ thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..9b26a5bbbc2cbee07572abccdebf8393c0943072
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..c27e01574b46fa76d820c9d5a31a6b5531e9d3d1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Find all the available input interfaces and try to initialize them.
+
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['InputInterface']
+
+import os
+import glob
+import logging
+from ..inputreaderinterface import InputReaderInterface
+
+logger = logging.getLogger(__name__)
+
+found_interfaces = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) + "/[A-Za-z]*.py")]
+if len(found_interfaces) == 0:
+ found_interfaces = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) +
+ "/[A-Za-z]*.pyc")]
+
+logger.info("Found interfaces: {}".format(found_interfaces))
+
+initialized_interfaces = []
+available_interfaces = []
+
+for interface in found_interfaces:
+ try:
+ module = __import__(interface, globals(), locals(), [interface], -1)
+ main_name = getattr(module, "MODULE_MAIN")
+ initialized_interfaces.append(getattr(module, main_name)())
+ logger.info("Successfully initialized [{}]".format(interface))
+ except Exception as e:
+ logger.info("Could not initialize [{}]: {}".format(interface, e))
+
+def devices():
+ # Todo: Support rescanning and adding/removing devices
+ if len(available_interfaces) == 0:
+ for reader in initialized_interfaces:
+ devs = reader.devices()
+ for dev in devs:
+ available_interfaces.append(InputInterface(dev["name"],
+ dev["id"],
+ reader))
+ return available_interfaces
+
+class InputInterface(InputReaderInterface):
+ def __init__(self, dev_name, dev_id, dev_reader):
+ super(InputInterface, self).__init__(dev_name, dev_id, dev_reader)
+
+ # These devices cannot be mapped and configured
+ self.supports_mapping = False
+
+ # Ask the reader if it wants to limit
+ # roll/pitch/yaw/thrust for all devices
+ self.limit_rp = dev_reader.limit_rp
+ self.limit_thrust = dev_reader.limit_thrust
+ self.limit_yaw = dev_reader.limit_yaw
+
+ def open(self):
+ self._reader.open(self.id)
+
+ def close(self):
+ self._reader.close(self.id)
+
+ def read(self, include_raw=False):
+ mydata = self._reader.read(self.id)
+ # Merge interface returned data into InputReader Data Item
+ for key in mydata.keys():
+ self.data.set(key, mydata[key])
+
+ return self.data
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..0b8ff01cd5178a884db66a9b124ccbe1efb1dace
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.py
new file mode 100755
index 0000000000000000000000000000000000000000..80c76d0725bf21c73a746fd5c98fe8677cca467f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.py
@@ -0,0 +1,149 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
+# USA.
+
+"""
+Leap Motion reader for controlling the Crazyflie. Note that this reader needs
+the Leap Motion SDK to be manually copied. See lib/leapsdk/__init__.py for
+more info.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LeapmotionReader']
+
+try:
+ import leapsdk.Leap as Leap
+ from leapsdk.Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture
+except Exception as e:
+ raise Exception("Leap Motion library probably not installed ({})".format(e))
+
+import logging
+import time
+
+logger = logging.getLogger(__name__)
+
+MODULE_MAIN = "LeapmotionReader"
+MODULE_NAME = "Leap Motion"
+
+class LeapListener(Leap.Listener):
+
+ def set_data_callback(self, callback):
+ self._dcb = callback
+ self._nbr_of_fingers = 0
+
+ def on_init(self, controller):
+ logger.info("Initialized")
+
+ def on_connect(self, controller):
+ logger.info("Connected")
+
+ def on_disconnect(self, controller):
+ # Note: not dispatched when running in a debugger.
+ logger.info("Disconnected")
+
+ def on_exit(self, controller):
+ logger.info("Exited")
+
+ def nbr_of_fingers(self):
+ return self._nbr_of_fingers
+
+ def on_frame(self, controller):
+ # Get the most recent frame and report some basic information
+ frame = controller.frame()
+ data = {"roll": 0, "pitch": 0, "yaw": 0, "thrust": 0}
+ #logger.info("Frame id: %d, timestamp: %d, hands: %d, fingers: %d, tools: %d, gestures: %d" % (
+ # frame.id, frame.timestamp, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures())))
+ if not frame.hands.is_empty:
+ # Get the first hand
+ hand = frame.hands[0]
+
+ normal = hand.palm_normal
+ direction = hand.direction
+ # Pich and roll are mixed up...
+
+ if len(hand.fingers) >= 4:
+ data["roll"] = -direction.pitch * Leap.RAD_TO_DEG / 30.0
+ data["pitch"] = -normal.roll * Leap.RAD_TO_DEG / 30.0
+ data["yaw"] = direction.yaw * Leap.RAD_TO_DEG / 70.0
+ data["thrust"] = (hand.palm_position[1] - 80)/150.0 # Use the elevation of the hand for thrust
+
+ if data["thrust"] < 0.0:
+ data["thrust"] = 0.0
+ if data["thrust"] > 1.0:
+ data["thrust"] = 1.0
+
+ self._dcb(data)
+
+class LeapmotionReader:
+ """Used for reading data from input devices using the PyGame API."""
+ def __init__(self):
+ #pygame.init()
+ self._ts = 0
+ self._listener = LeapListener()
+ self._listener.set_data_callback(self.leap_callback)
+ self._controller = Leap.Controller()
+ self._controller.add_listener(self._listener)
+ self.name = MODULE_NAME
+
+ self.limit_rp = True
+ self.limit_thrust = True
+ self.limit_yaw = True
+
+ self.data = {"roll": 0.0, "pitch": 0.0, "yaw": 0.0,
+ "thrust": -1.0, "estop": False, "exit": False,
+ "althold": False, "alt1": False, "alt2": False,
+ "pitchNeg": False, "rollNeg": False,
+ "pitchPos": False, "rollPos": False}
+ logger.info("Initialized Leap")
+
+ def open(self, deviceId):
+ """Initialize the reading and open the device with deviceId and set the mapping for axis/buttons using the
+ inputMap"""
+ return
+
+ def leap_callback(self, data):
+ for k in data.keys():
+ self.data[k] = data[k]
+
+ def read(self, id):
+ """Read input from the selected device."""
+ return self.data
+
+ def close(self, id):
+ return
+
+ def devices(self):
+ """List all the available devices."""
+ dev = []
+
+ # According to API doc only 0 or 1 devices is supported
+ #logger.info("Devs: {}".format(self._controller.is_connected))
+ if self._controller.is_connected:
+ dev.append({"id": 0, "name": "Leapmotion"})
+
+ return dev
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..7715c47e3d587d68a8815f5c15244d4fe9273e17
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/leapmotion.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.py
new file mode 100755
index 0000000000000000000000000000000000000000..290e96284a16f92eaf41a716c86ef6f2e8d74c70
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.py
@@ -0,0 +1,141 @@
+import logging
+from threading import Thread
+import time
+
+try:
+ import cwiid
+except ImportError as e:
+ raise Exception("Missing cwiid (wiimote) driver {}".format(e))
+
+logger = logging.getLogger(__name__)
+
+MODULE_MAIN = "WiimoteReader"
+MODULE_NAME = "WII"
+
+class _Reader(object):
+ # needs attributes:
+ # - name
+ # - limit_rp
+ # - limit_thrust
+ # - limit_yaw
+ # - open
+ def devices(self):
+ """List all the available connections"""
+ raise NotImplemented()
+ def open(self, device_id):
+ """Initialize the reading and open the device with deviceId and set the mapping for axis/buttons using the
+ inputMap"""
+ return
+ def close(self, device_id):
+ return
+ def read(self, device_id):
+ """Read input from the selected device."""
+ raise NotImplemented()
+
+
+TWO = 1
+ONE = 2
+B = 4
+A = 8
+MINUS = 16
+HOME = 128
+LEFT = 256
+RIGHT = 512
+DOWN = 1024
+UP = 2048
+PLUS = 4096
+
+
+class HandleWiimote(Thread):
+ def __init__(self, reader, wii, *args):
+ super(HandleWiimote, self).__init__(*args)
+ self.reader = reader
+ self.wii = wii
+ self.daemon = True
+
+ def run(self):
+ logger.info("\n\nRUNNING THREAD!\n\n\n")
+ t_delta = 100
+ move_delta = .3
+ max_move = 8
+ min_sample = .1
+ max_sample = .01
+ sample = min_sample
+ while True:
+ button = self.wii.state['buttons']
+ pitch = self.reader.data['pitch']
+ roll = self.reader.data['roll']
+ if button & ONE:
+ logger.info("UP!! {}".format(self.reader.data['thrust']))
+ self.reader.data['thrust'] += t_delta
+ if button & TWO:
+ logger.info("DOWN!! {}".format(self.reader.data['thrust']))
+ self.reader.data['thrust'] -= t_delta * 3
+ if self.reader.data['thrust'] < -1:
+ self.reader.data['thrust'] = -1
+ if button & RIGHT:
+ logger.info("RIGHT PITCH {}".format(pitch))
+ self.reader.data['pitch'] = min(max_move, pitch + move_delta)
+ if button & LEFT:
+ logger.info("LEFT PITCH {}".format(pitch))
+ self.reader.data['pitch'] = max(-max_move, pitch - move_delta)
+ if button & UP:
+ logger.info("UP ROLL {}".format(roll))
+ self.reader.data['roll'] = max(-max_move, roll - move_delta)
+ if button & DOWN:
+ logger.info("DOWN ROLL {}".format(roll))
+ self.reader.data['roll'] = min(max_move, roll + move_delta)
+ if button & B:
+ # KILL
+ self.reader.data['thrust'] = -1
+
+ if button:
+ sample = max(max_sample, sample/3)
+ else:
+ sample = min(min_sample, sample*3)
+ self.adjust()
+ time.sleep(sample)
+
+ def adjust(self):
+ pitch = self.reader.data['pitch']
+ if pitch > 1.2:
+ self.reader.data['pitch'] -= 1
+ elif pitch < -1.2:
+ self.reader.data['pitch'] += 1
+ else:
+ self.reader.data['pitch'] = 0
+ roll = self.reader.data['roll']
+ if roll > 1.2:
+ self.reader.data['roll'] -= 1
+ elif roll < -1.2:
+ self.reader.data['roll'] += 1
+ else:
+ self.reader.data['roll'] = 0
+
+class WiimoteReader(_Reader):
+ name = MODULE_NAME
+
+ def __init__(self):
+ self.limit_rp = False
+ self.limit_thrust = False
+ self.limit_yaw = False
+
+ print "Press 1 + 2 to connect wii"
+ time.sleep(1)
+ self.wm = cwiid.Wiimote()
+ self.wm.rpt_mode = cwiid.RPT_BTN
+ logger.info("FOUND WIIMOTE")
+ self.data = {"roll": 0.0, "pitch": 0.0, "yaw": 0.0,
+ "thrust": -1.0, "estop": False, "exit": False,
+ "althold": False, "alt1": False, "alt2": False,
+ "pitchNeg": False, "rollNeg": False,
+ "pitchPos": False, "rollPos": False}
+ self.wii_thread = HandleWiimote(self, self.wm)
+ self.wii_thread.start()
+
+ def read(self, dev_id):
+ return self.data
+
+ def devices(self):
+ """List all the available connections"""
+ return [{"id": 0, "name": "WII@{}".format(self.wm)}]
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..a7367d572ff1f96996c426106d5de2b58bd85b78
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/wiimote.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.py
new file mode 100755
index 0000000000000000000000000000000000000000..f12f702d1c908342a308bc2fe7cf5ddc0b9ff86f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.py
@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
+# USA.
+
+"""
+Input interface that supports receiving commands via ZMQ.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['ZMQReader']
+
+try:
+ import zmq
+except Exception as e:
+ raise Exception("ZMQ library probably not installed ({})".format(e))
+
+from cfclient.utils.config import Config
+if not Config().get("enable_zmq_input"):
+ raise Exception("ZMQ input disabled in config file")
+
+import logging
+import time
+import pprint
+from threading import Thread
+
+ZMQ_PULL_PORT = 1024 + 188
+
+logger = logging.getLogger(__name__)
+
+MODULE_MAIN = "ZMQReader"
+MODULE_NAME = "ZMQ"
+
+class _PullReader(Thread):
+
+ def __init__(self, receiver, callback, *args):
+ super(_PullReader, self).__init__(*args)
+ self._receiver = receiver
+ self._cb = callback
+ self.daemon = True
+
+ def run(self):
+ while True:
+ self._cb(self._receiver.recv_json())
+
+class ZMQReader:
+ """Used for reading data from input devices using the PyGame API."""
+ def __init__(self):
+ context = zmq.Context()
+ receiver = context.socket(zmq.PULL)
+ self._bind_addr = "tcp://127.0.0.1:{}".format(ZMQ_PULL_PORT)
+ # If the port is already bound an exception will be thrown
+ # and caught in the initialization of the readers and handled.
+ receiver.bind(self._bind_addr)
+ logger.info("Biding ZMQ at {}".format(self._bind_addr))
+
+ self.name = MODULE_NAME
+
+ self.limit_rp = False
+ self.limit_thrust = False
+ self.limit_yaw = False
+
+ self.data = {"roll": 0.0, "pitch": 0.0, "yaw": 0.0,
+ "thrust": -1.0, "estop": False, "exit": False,
+ "althold": False, "alt1": False, "alt2": False,
+ "pitchNeg": False, "rollNeg": False,
+ "pitchPos": False, "rollPos": False}
+
+ logger.info("Initialized ZMQ")
+
+ self._receiver_thread = _PullReader(receiver, self._cmd_callback)
+ self._receiver_thread.start()
+
+ def _cmd_callback(self, cmd):
+ for k in cmd["ctrl"].keys():
+ self.data[k] = cmd["ctrl"][k]
+
+ def open(self, device_id):
+ """Initialize the reading and open the device with deviceId and set the mapping for axis/buttons using the
+ inputMap"""
+ return
+
+ def read(self, device_id):
+ """Read input from the selected device."""
+
+ return self.data
+
+ def close(self, device_id):
+ return
+
+ def devices(self):
+ """List all the available connections"""
+ # As a temporary workaround we always say we have ZMQ
+ # connected. If it's not connected, there's just no data.
+ return [{"id": 0, "name": "ZMQ@{}".format(self._bind_addr)}]
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..e1193a55587204f671f4fe287d21eeaa117ab9f1
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputinterfaces/zmqpull.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.py
new file mode 100755
index 0000000000000000000000000000000000000000..7e41cd731260a78202cf7cd760c278869c4008c3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.py
@@ -0,0 +1,258 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Interface for reading input devices and interfaces
+"""
+
+from time import time
+import logging
+
+logger = logging.getLogger(__name__)
+
+
+class _ToggleState(dict):
+ def __getattr__(self, attr):
+ return self.get(attr)
+
+ __setattr__ = dict.__setitem__
+ __delattr__ = dict.__delitem__
+
+
+class InputData:
+ def __init__(self):
+ #self._toggled = {}
+ self._axes = ("roll", "pitch", "yaw", "thrust")
+ self._buttons = ("alt1", "alt2", "estop", "exit", "pitchNeg",
+ "pitchPos", "rollNeg", "rollPos", "althold",
+ "muxswitch")
+ for axis in self._axes:
+ self.__dict__[axis] = 0.0
+ self.toggled = _ToggleState()
+ self._prev_btn_values = {}
+ for button in self._buttons:
+ self.__dict__[button] = False
+ self.toggled[button] = False
+ self._prev_btn_values[button] = False
+
+ def get_all_indicators(self):
+ return self._axes + self._buttons
+
+ def _check_toggle(self, key, data):
+ if not key in self._prev_btn_values:
+ self._prev_btn_values[key] = data
+ elif self._prev_btn_values[key] != data:
+ self._prev_btn_values[key] = data
+ return True
+ return False
+
+ def reset_axes(self):
+ for axis in self._axes:
+ self.__dict__[axis] = 0.0
+
+ def reset_buttons(self):
+ for button in self._buttons:
+ self.__dict__[button] = False
+
+ def set(self, name, value):
+ try:
+ if name in self._buttons:
+ if self._check_toggle(name, value):
+ self.toggled[name] = True
+ else:
+ self.toggled[name] = False
+ except KeyError:
+ pass
+ self.__dict__[name] = value
+
+ def get(self, name):
+ return self.__dict__[name]
+
+
+class InputReaderInterface(object):
+ def __init__(self, dev_name, dev_id, dev_reader):
+ """Initialize the reader"""
+ # Set if the device supports mapping and can be configured
+ self.supports_mapping = True
+
+ # Set if the MUX should automatically limit roll/pitch/thrust/yaw
+ # according to the settings in the UI
+ self.limit_rp = True
+ self.limit_thrust = True
+ self.limit_yaw = True
+
+ self.input = None
+
+ self._reader = dev_reader
+ self.id = dev_id
+ self.name = dev_name
+ self.input_map = None
+ self.input_map_name = ""
+ self.data = None
+ self._prev_pressed = None
+ self.reader_name = dev_reader.name
+
+ self.data = InputData()
+
+ # Stateful things
+ self._old_thrust = 0
+ self._old_raw_thrust = 0
+ self._old_alt_hold = False
+
+ self._prev_thrust = 0
+ self._last_time = 0
+ # How low you have to pull the thrust to bypass the slew-rate (0-100%)
+ self.thrust_stop_limit = -90
+
+ def open(self):
+ """Initialize the reading and open the device with deviceId and set the
+ mapping for axis/buttons using the inputMap"""
+ return
+
+ def read(self):
+ """Read input from the selected device."""
+ return None
+
+ def close(self):
+ return
+
+ @staticmethod
+ def devices():
+ """List all the available devices."""
+ return []
+
+ def _cap_rp(self, rp):
+ ret = rp * self.input.max_rp_angle
+ if ret > self.input.max_rp_angle:
+ ret = self.input.max_rp_angle
+ elif ret < -1 * self.input.max_rp_angle:
+ ret = -1 * self.input.max_rp_angle
+
+ return ret
+
+ def _scale_rp(self, roll, pitch):
+ return [self._cap_rp(roll), self._cap_rp(pitch)]
+
+ def _scale_and_deadband_yaw(self, yaw):
+ return InputReaderInterface.deadband(yaw, 0.2) * self.input.max_yaw_rate
+
+ def _limit_thrust(self, thrust, althold, emergency_stop):
+ # Thust limiting (slew, minimum and emergency stop)
+ if self.input.springy_throttle:
+ if althold and self.input.has_pressure_sensor:
+ thrust = int(round(InputReaderInterface.deadband(thrust, 0.2)
+ * 32767 + 32767)) # Convert to uint16
+ else:
+ # Scale the thrust to percent (it's between 0 and 1)
+ thrust *= 100
+
+ # The default action is to just use the thrust...
+ limited_thrust = thrust
+ if limited_thrust > self.input.max_thrust:
+ limited_thrust = self.input.max_thrust
+
+ # ... but if we are lowering the thrust, check the limit
+ if self._prev_thrust > thrust >= self.thrust_stop_limit and \
+ not emergency_stop:
+ # If we are above the limit, then don't use the slew...
+ if thrust > self.input.thrust_slew_limit:
+ limited_thrust = thrust
+ else:
+ # ... but if we are below first check if we "entered"
+ # the limit, then set it to the limit
+ if self._prev_thrust > self.input.thrust_slew_limit:
+ limited_thrust = self.input.thrust_slew_limit
+ else:
+ # If we are "inside" the limit, then lower
+ # according to the rate we have set each iteration
+ lowering = (time() - self._last_time) * \
+ self.input.thrust_slew_rate
+ limited_thrust = self._prev_thrust - lowering
+ elif emergency_stop or thrust < self.thrust_stop_limit:
+ # If the thrust have been pulled down or the
+ # emergency stop has been activated then bypass
+ # the slew and force 0
+ self._prev_thrust = 0
+ limited_thrust = 0
+
+ # For the next iteration set the previous thrust to the limited
+ # one (will be the slewed thrust if we are slewing)
+ self._prev_thrust = limited_thrust
+
+ # Lastly make sure we're following the "minimum" thrust setting
+ if limited_thrust < self.input.min_thrust:
+ self._prev_thrust = 0
+ limited_thrust = 0
+
+ self._last_time = time()
+
+ thrust = limited_thrust
+ else:
+ thrust = thrust / 2 + 0.5
+ if althold and self.input.has_pressure_sensor:
+ thrust = 32767
+ else:
+ if thrust < -0.90 or emergency_stop:
+ thrust = 0
+ else:
+ thrust = self.input.min_thrust + thrust * (
+ self.input.max_thrust -
+ self.input.min_thrust)
+ if (self.input.thrust_slew_enabled and
+ self.input.thrust_slew_limit > thrust and not
+ emergency_stop):
+ if self._old_thrust > self.input.thrust_slew_limit:
+ self._old_thrust = self.input.thrust_slew_limit
+ if thrust < (self._old_thrust -
+ (self.input.thrust_slew_rate / 100)):
+ thrust = self._old_thrust - \
+ self.input.thrust_slew_rate / 100
+ if thrust < -1 or thrust < self.input.min_thrust:
+ thrust = 0
+
+ self._old_thrust = thrust
+ self._old_raw_thrust = thrust
+ return thrust
+
+ def set_alt_hold_available(self, available):
+ """Set if altitude hold is available or not (depending on HW)"""
+ self.input._has_pressure_sensor = available
+
+ def enable_alt_hold(self, althold):
+ """Enable or disable altitude hold"""
+ self._old_alt_hold = althold
+
+ @staticmethod
+ def deadband(value, threshold):
+ if abs(value) < threshold:
+ value = 0
+ elif value > 0:
+ value -= threshold
+ elif value < 0:
+ value += threshold
+ return value / (1 - threshold)
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..b768cf698b267463801adcd5ac995978e6ad369c
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaderinterface.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..11d1a8feeb9b9538b0c18c3a762c3be23c636308
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.py
@@ -0,0 +1,146 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Find all the available input readers and try to import them.
+
+To create a new input device reader drop a .py file into this
+directory and it will be picked up automatically.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['InputDevice']
+
+import os
+import glob
+import logging
+from ..inputreaderinterface import InputReaderInterface
+
+logger = logging.getLogger(__name__)
+
+found_readers = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) + "/[A-Za-z]*.py")]
+if len(found_readers) == 0:
+ found_readers = [os.path.splitext(os.path.basename(f))[0] for
+ f in glob.glob(os.path.dirname(__file__) +
+ "/[A-Za-z]*.pyc")]
+
+logger.info("Found readers: {}".format(found_readers))
+
+initialized_readers = []
+available_devices = []
+
+for reader in found_readers:
+ try:
+ module = __import__(reader, globals(), locals(), [reader], -1)
+ main_name = getattr(module, "MODULE_MAIN")
+ initialized_readers.append(getattr(module, main_name)())
+ logger.info("Successfully initialized [{}]".format(reader))
+ except Exception as e:
+ logger.info("Could not initialize [{}]: {}".format(reader, e))
+
+def devices():
+ # Todo: Support rescanning and adding/removing devices
+ if len(available_devices) == 0:
+ for r in initialized_readers:
+ devs = r.devices()
+ for dev in devs:
+ available_devices.append(InputDevice(dev["name"],
+ dev["id"],
+ r))
+ return available_devices
+
+
+class InputDevice(InputReaderInterface):
+ def __init__(self, dev_name, dev_id, dev_reader):
+ super(InputDevice, self).__init__(dev_name, dev_id, dev_reader)
+
+ # All devices supports mapping (and can be configured)
+ self.supports_mapping = True
+
+ # Limit roll/pitch/yaw/thrust for all devices
+ self.limit_rp = True
+ self.limit_thrust = True
+ self.limit_yaw = True
+
+ def open(self):
+ # TODO: Reset data?
+ self._reader.open(self.id)
+
+ def close(self):
+ self._reader.close(self.id)
+
+ def read(self, include_raw=False):
+ [axis, buttons] = self._reader.read(self.id)
+
+ # To support split axis we need to zero all the axis
+ self.data.reset_axes()
+
+ i = 0
+ for a in axis:
+ index = "Input.AXIS-%d" % i
+ try:
+ if self.input_map[index]["type"] == "Input.AXIS":
+ key = self.input_map[index]["key"]
+ axisvalue = a + self.input_map[index]["offset"]
+ axisvalue = axisvalue / self.input_map[index]["scale"]
+ self.data.set(key, axisvalue + self.data.get(key))
+ except (KeyError, TypeError):
+ pass
+ i += 1
+
+ # Workaround for fixing issues during mapping (remapping buttons while
+ # they are pressed.
+ self.data.reset_buttons()
+
+ i = 0
+ for b in buttons:
+ index = "Input.BUTTON-%d" % i
+ try:
+ if self.input_map[index]["type"] == "Input.BUTTON":
+ key = self.input_map[index]["key"]
+ self.data.set(key, True if b == 1 else False)
+ except (KeyError, TypeError):
+ # Button not mapped, ignore..
+ pass
+ i += 1
+
+ if self.limit_rp:
+ [self.data.roll, self.data.pitch] = self._scale_rp(self.data.roll,
+ self.data.pitch)
+ if self.limit_thrust:
+ self.data.thrust = self._limit_thrust(self.data.thrust,
+ self.data.althold,
+ self.data.estop)
+ if self.limit_yaw:
+ self.data.yaw = self._scale_and_deadband_yaw(self.data.yaw)
+
+ if include_raw:
+ return [axis, buttons, self.data]
+ else:
+ return self.data
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..fbb91527005546476a12468b4d18d99d6b80b392
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.py
new file mode 100755
index 0000000000000000000000000000000000000000..a0fee1ba6aa35baf1dd01edc4084821ce889d769
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.py
@@ -0,0 +1,227 @@
+# -*- coding: utf8 -*-
+# ||
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Linux joystick driver using the Linux input_joystick subsystem. Requires sysfs
+to be mounted on /sys and /dev/input/js* to be readable.
+
+This module is very linux specific but should work on any CPU platform
+"""
+
+import sys
+if not sys.platform.startswith('linux'):
+ raise Exception("Only supported on Linux")
+
+import struct
+import glob
+import os
+import ctypes
+import fcntl
+import logging
+
+logger = logging.getLogger(__name__)
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Joystick']
+
+JS_EVENT_FMT = "@IhBB"
+JE_TIME = 0
+JE_VALUE = 1
+JE_TYPE = 2
+JE_NUMBER = 3
+
+JS_EVENT_BUTTON = 0x001
+JS_EVENT_AXIS = 0x002
+JS_EVENT_INIT = 0x080
+
+#ioctls
+JSIOCGAXES = 0x80016a11
+JSIOCGBUTTONS = 0x80016a12
+
+MODULE_MAIN = "Joystick"
+MODULE_NAME = "linuxjsdev"
+
+class JEvent(object):
+ """
+ Joystick event class. Encapsulate single joystick event.
+ """
+ def __init__(self, evt_type, number, value):
+ self.type = evt_type
+ self.number = number
+ self.value = value
+
+ def __repr__(self):
+ return "JEvent(type={}, number={}, value={})".format(self.type,
+ self.number, self.value)
+
+#Constants
+TYPE_BUTTON = 1
+TYPE_AXIS = 2
+
+class _JS():
+ def __init__(self, num, name):
+ self.num = num
+ self.name = name
+ self._f_name = "/dev/input/js{}".format(num)
+ self._f = None
+
+ self.opened = False
+ self.buttons = []
+ self.axes = []
+ self._prev_pressed = {}
+
+ def open(self):
+ if self._f:
+ raise Exception("{} at {} is already "
+ "opened".format(self.name, self._f_name))
+
+ self._f = open("/dev/input/js{}".format(self.num), "r")
+ fcntl.fcntl(self._f.fileno(), fcntl.F_SETFL, os.O_NONBLOCK)
+
+ #Get number of axis and button
+ val = ctypes.c_int()
+ if fcntl.ioctl(self._f.fileno(), JSIOCGAXES, val) != 0:
+ self._f.close()
+ self._f = None
+ raise Exception("Failed to read number of axes")
+
+ self.axes = list(0 for i in range(val.value))
+ if fcntl.ioctl(self._f.fileno(), JSIOCGBUTTONS, val) != 0:
+ self._f.close()
+ self._f = None
+ raise Exception("Failed to read number of axes")
+
+ self.buttons = list(0 for i in range(val.value))
+ self.__initvalues()
+
+ def close(self):
+ """Open the joystick device"""
+ if not self._f:
+ return
+
+ logger.info("Closed {} ({})".format(self.name, self.num))
+
+ self._f.close()
+ self._f = None
+
+ def __initvalues(self):
+ """Read the buttons and axes initial values from the js device"""
+ for _ in range(len(self.axes) + len(self.buttons)):
+ data = self._f.read(struct.calcsize(JS_EVENT_FMT))
+ jsdata = struct.unpack(JS_EVENT_FMT, data)
+ self.__updatestate(jsdata)
+
+ def __updatestate(self, jsdata):
+ """Update the internal absolute state of buttons and axes"""
+ if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0:
+ self.axes[jsdata[JE_NUMBER]] = jsdata[JE_VALUE] / 32768.0
+ elif jsdata[JE_TYPE] & JS_EVENT_BUTTON != 0:
+ self.buttons[jsdata[JE_NUMBER]] = jsdata[JE_VALUE]
+
+ def __decode_event(self, jsdata):
+ """ Decode a jsdev event into a dict """
+ #TODO: Add timestamp?
+ if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0:
+ return JEvent(evt_type=TYPE_AXIS,
+ number=jsdata[JE_NUMBER],
+ value=jsdata[JE_VALUE] / 32768.0)
+ if jsdata[JE_TYPE] & JS_EVENT_BUTTON != 0:
+ return JEvent(evt_type=TYPE_BUTTON,
+ number=jsdata[JE_NUMBER],
+ value=jsdata[JE_VALUE] / 32768.0)
+
+ def _read_all_events(self):
+ """Consume all the events queued up in the JS device"""
+ try:
+ while True:
+ data = self._f.read(struct.calcsize(JS_EVENT_FMT))
+ jsdata = struct.unpack(JS_EVENT_FMT, data)
+ self.__updatestate(jsdata)
+ except IOError as e:
+ if e.errno != 11:
+ logger.info(str(e))
+ self._f.close()
+ self._f = None
+ raise IOError("Device has been disconnected")
+ except ValueError:
+ # This will happen if I/O operations are done on a closed device,
+ # which is the case when you first close and then open the device
+ # while switching device. But, in order for SDL2 to work on Linux
+ # (for debugging) the device needs to be closed before it's opened.
+ # This is the workaround to make both cases work.
+ pass
+
+ def read(self):
+ """ Returns a list of all joystick event since the last call """
+ if not self._f:
+ raise Exception("Joystick device not opened")
+
+ self._read_all_events()
+
+ return [self.axes, self.buttons]
+
+
+class Joystick():
+ """
+ Linux jsdev implementation of the Joystick class
+ """
+
+ def __init__(self):
+ self.name = MODULE_NAME
+ self._js = {}
+ self._devices = []
+
+ def devices(self):
+ """
+ Returns a dict with device_id as key and device name as value of all
+ the detected devices (result is cached once one or more device are
+ found).
+ """
+
+ if len(self._devices) == 0:
+ syspaths = glob.glob("/sys/class/input/js*")
+
+ for path in syspaths:
+ device_id = int(os.path.basename(path)[2:])
+ with open(path + "/device/name") as namefile:
+ name = namefile.read().strip()
+ self._js[device_id] = _JS(device_id, name)
+ self._devices.append({"id": device_id, "name": name})
+
+ return self._devices
+
+ def open(self, device_id):
+ """
+ Open the joystick device. The device_id is given by available_devices
+ """
+ self._js[device_id].open()
+
+ def close(self, device_id):
+ """Open the joystick device"""
+ self._js[device_id].close()
+
+ def read(self, device_id):
+ """ Returns a list of all joystick event since the last call """
+ return self._js[device_id].read()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..4816fdfea45669fd14e80ae7cff76c50a9c7465c
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/linuxjsdev.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.py
new file mode 100755
index 0000000000000000000000000000000000000000..f998ad8e4f48e839924aa682849206a10e8a746b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.py
@@ -0,0 +1,189 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
+# USA.
+
+"""
+Driver for reading data from the PySDL2 API. Used from Inpyt.py for reading
+input data.
+"""
+
+import sys
+if sys.platform.startswith('linux'):
+ raise Exception("No SDL2 support on Linux")
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PySDL2Reader']
+
+import sdl2
+import sdl2.ext
+import sdl2.hints
+from threading import Thread
+from Queue import Queue
+import time
+import logging
+
+logger = logging.getLogger(__name__)
+
+MODULE_MAIN = "PySDL2Reader"
+MODULE_NAME = "PySDL2"
+
+class _SDLEventDispatcher(Thread):
+ """Wrapper to read all SDL2 events from the global queue and distribute
+ them to the different devices"""
+ def __init__(self, callback):
+ Thread.__init__(self)
+ self._callback = callback
+ self.daemon = True
+ # SDL2 will Seg Fault on Linux if you read events after you
+ # have closed a device (and without opening a new one). Even if you
+ # have two devices open, it will crash after one.
+ self.enable = False
+
+ def run(self):
+ while True:
+ if self.enable:
+ for ev in sdl2.ext.get_events():
+ try:
+ if self._callback:
+ self._callback(ev.jdevice.which, ev)
+ except AttributeError:
+ pass
+ time.sleep(0.01)
+
+
+class _JS():
+ """Wrapper for one input device"""
+ def __init__(self, sdl_index, sdl_id, name):
+ self.axes = []
+ self.buttons = []
+ self.name = MODULE_NAME
+ self._j = None
+ self._btn_count = 0
+ self._id = sdl_id
+ self._index = sdl_index
+ self._name = name
+ self._event_queue = Queue()
+
+ def open(self):
+ self._j = sdl2.SDL_JoystickOpen(self._index)
+ self._btn_count = sdl2.SDL_JoystickNumButtons(self._j)
+
+ self.axes = list(0 for i in range(sdl2.SDL_JoystickNumAxes(self._j)))
+ self.buttons = list(0 for i in range(sdl2.SDL_JoystickNumButtons(
+ self._j) + 4))
+
+ def close(self):
+ if self._j:
+ sdl2.joystick.SDL_JoystickClose(self._j)
+ self._j = None
+
+ def _set_fake_hat_button(self, btn=None):
+ self.buttons[self._btn_count] = 0
+ self.buttons[self._btn_count+1] = 0
+ self.buttons[self._btn_count+2] = 0
+ self.buttons[self._btn_count+3] = 0
+
+ if btn:
+ self.buttons[self._btn_count+btn] = 1
+
+ def add_event(self, event):
+ self._event_queue.put(event)
+
+ def read(self):
+ while not self._event_queue.empty():
+ e = self._event_queue.get_nowait()
+ if e.type == sdl2.SDL_JOYAXISMOTION:
+ self.axes[e.jaxis.axis] = e.jaxis.value / 32767.0
+
+ if e.type == sdl2.SDL_JOYBUTTONDOWN:
+ self.buttons[e.jbutton.button] = 1
+
+ if e.type == sdl2.SDL_JOYBUTTONUP:
+ self.buttons[e.jbutton.button] = 0
+
+ if e.type == sdl2.SDL_JOYHATMOTION:
+ if e.jhat.value == sdl2.SDL_HAT_CENTERED:
+ self._set_fake_hat_button()
+ elif e.jhat.value == sdl2.SDL_HAT_UP:
+ self._set_fake_hat_button(0)
+ elif e.jhat.value == sdl2.SDL_HAT_DOWN:
+ self._set_fake_hat_button(1)
+ elif e.jhat.value == sdl2.SDL_HAT_LEFT:
+ self._set_fake_hat_button(2)
+ elif e.jhat.value == sdl2.SDL_HAT_RIGHT:
+ self._set_fake_hat_button(3)
+ return [self.axes, self.buttons]
+
+class PySDL2Reader():
+ """Used for reading data from input devices using the PySDL2 API."""
+ def __init__(self):
+ sdl2.SDL_Init(sdl2.SDL_INIT_VIDEO | sdl2.SDL_INIT_JOYSTICK)
+ sdl2.SDL_SetHint(sdl2.hints.SDL_HINT_JOYSTICK_ALLOW_BACKGROUND_EVENTS,
+ "1")
+ sdl2.ext.init()
+ self._js = {}
+ self.name = MODULE_NAME
+ self._event_dispatcher = _SDLEventDispatcher(self._dispatch_events)
+ self._event_dispatcher.start()
+ self._devices = []
+
+ def open(self, device_id):
+ """Initialize the reading and open the device with deviceId and set
+ the mapping for axis/buttons using the inputMap"""
+ self._event_dispatcher.enable = True
+ self._js[device_id].open()
+
+ def close(self, device_id):
+ """Close the device"""
+ self._event_dispatcher.enable = False
+ self._js[device_id].close()
+
+ def read(self, device_id):
+ """Read input from the selected device."""
+ return self._js[device_id].read()
+
+ def _dispatch_events(self, device_id, event):
+ self._js[device_id].add_event(event)
+
+ def devices(self):
+ """List all the available devices."""
+ logger.info("Looking for devices")
+ names = []
+ if len(self._devices) == 0:
+ nbrOfInputs = sdl2.joystick.SDL_NumJoysticks()
+ logger.info("Found {} devices".format(nbrOfInputs))
+ for sdl_index in range(0, nbrOfInputs):
+ j = sdl2.joystick.SDL_JoystickOpen(sdl_index)
+ name = sdl2.joystick.SDL_JoystickName(j)
+ if names.count(name) > 0:
+ name = "{0} #{1}".format(name, names.count(name) + 1)
+ sdl_id = sdl2.joystick.SDL_JoystickInstanceID(j)
+ self._devices.append({"id": sdl_id, "name": name})
+ self._js[sdl_id] = _JS(sdl_index, sdl_id, name)
+ names.append(name)
+ sdl2.joystick.SDL_JoystickClose(j)
+ return self._devices
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..141afec89ad6dd1d4462cddb511db7cfad27c97d
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/inputreaders/pysdl2.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..402c4d87faffbc390d9099f6aa6f955e6d412135
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+"""
+The mux is used to open one or more devices and mix the inputs from all
+of them into one "input" for the Crazyflie and UI.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['InputMux']
+
+import logging
+
+logger = logging.getLogger(__name__)
+
+
+class InputMux(object):
+ def __init__(self, input_layer):
+ self._devs = {"Device": None}
+ self.name = "N/A"
+ self.input = input_layer
+
+ def _open_new_device(self, dev, role):
+ # Silently close device if open as other role
+ for r in self._devs:
+ if self._devs[r]:
+ if self._devs[r] == dev:
+ self._devs[r].close()
+ self._devs[r] = None
+
+ if self._devs[role]:
+ self._devs[role].close()
+ self._devs[role] = dev
+ self._devs[role].open()
+
+ def supported_roles(self):
+ return self._devs.keys()
+
+ def add_device(self, dev, role):
+ logger.info("Adding device {} to MUX {}".format(dev.name, self.name))
+ self._open_new_device(dev, role)
+
+ def pause(self):
+ for d in [key for key in self._devs.keys() if self._devs[key]]:
+ self._devs[d].close()
+
+ def resume(self):
+ for d in [key for key in self._devs.keys() if self._devs[key]]:
+ self._devs[d].open()
+
+ def close(self):
+ """Close down the MUX and close all it's devices"""
+ for d in [key for key in self._devs.keys() if self._devs[key]]:
+ self._devs[d].close()
+ self._devs[d] = None
+
+ def read(self):
+ return None
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..ac147b6b3038f7b89e9ebfa6ad78bc2bf1f8f8e2
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.py
new file mode 100755
index 0000000000000000000000000000000000000000..d32965a1df44981b1dead2f9bdb920e4cc791564
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.py
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+"""
+This mux will only use one device and not mix it with any other input.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['NoMux']
+
+from . import InputMux
+import logging
+logger = logging.getLogger(__name__)
+
+
+class NoMux(InputMux):
+ def __init__(self, *args):
+ super(NoMux, self).__init__(*args)
+ self.name = "Normal"
+ self._devs = {"Device": None}
+
+ def read(self):
+ if self._devs["Device"]:
+ data = self._devs["Device"].read()
+
+ return data
+ else:
+ return None
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..295adbfe806ffd6abffdc7e855a0a8fdd5e6bced
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/nomux.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.py
new file mode 100755
index 0000000000000000000000000000000000000000..d4cccf3eaf7845d922d202c5633aa2b28a5d6e5b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+"""
+Mux for giving control to one device (slave/student) for all axis (roll/pitch/
+yaw/thrust) with the ability to take over all of them from a second device
+(master/teacher).
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['SelectiveMux']
+
+import logging
+
+from .takeoverselectivemux import TakeOverSelectiveMux
+
+logger = logging.getLogger(__name__)
+
+
+class TakeOverMux(TakeOverSelectiveMux):
+ def __init__(self, *args):
+ super(TakeOverMux, self).__init__(*args)
+ self.name = "Teacher (RPYT)"
+ self._muxing = {self._master: ("estop", "alt1", "alt2", "althold",
+ "exit"),
+ self._slave: ("roll", "pitch", "yaw", "thrust")}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..02296e8241ecfc48e3669cb25e0c5d3c2052fd54
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeovermux.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.py
new file mode 100755
index 0000000000000000000000000000000000000000..cde7af0282a1c285d0604f727cb84722a639c0d9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+# 02110-1301, USA.
+"""
+Mux for controlling roll/pitch from one device (slave/student) and the rest
+from a second device (master/teacher) with the possibility to take over
+roll/pitch as well.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['TakeOverSelectiveMux']
+
+import logging
+
+from . import InputMux
+
+logger = logging.getLogger(__name__)
+
+
+class TakeOverSelectiveMux(InputMux):
+ def __init__(self, *args):
+ super(TakeOverSelectiveMux, self).__init__(*args)
+ self._master = "Teacher"
+ self._slave = "Student"
+ self.name = "Teacher (RP)"
+ self._devs = {self._master: None, self._slave: None}
+
+ self._muxing = {self._master: ("thrust", "yaw", "estop", "alt1", "alt2",
+ "althold", "exit"),
+ self._slave: ("roll", "pitch")}
+
+ def read(self):
+ try:
+ if self._devs[self._master] and self._devs[self._slave]:
+ dm = self._devs[self._master].read()
+ ds = self._devs[self._slave].read()
+ if not dm.muxswitch:
+ for key in self._muxing[self._slave]:
+ dm.set(key, ds.get(key))
+
+ return dm
+ else:
+ return None
+
+ except Exception as e:
+ logger.warning(e)
+ return None
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..e24f03584f76f52794220b9cdbf47b64a85ffe4d
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/input/mux/takeoverselectivemux.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..f98770b26655e6ba6ee2e856607abb6defe8a526
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/linuxjsdev.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/linuxjsdev.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..45bc0a261684b6e4666f9b7509e4b3d94ebc7c92
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/joystick/linuxjsdev.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.py
new file mode 100755
index 0000000000000000000000000000000000000000..1e948b48a27cf3f4976e750a6f81aef0708328f9
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.py
@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+The input module that will read joysticks/input devices and send control set-
+points to the Crazyflie. It will also accept settings from the UI.
+
+This module can use different drivers for reading the input device data.
+Currently it can just use the PySdl2 driver but in the future there will be a
+Linux and Windows driver that can bypass PySdl2.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogVariable', 'LogConfigReader', 'LogConfigRemoveThis']
+
+import glob
+import json
+import logging
+import os
+import shutil
+import sys
+
+logger = logging.getLogger(__name__)
+
+#from PyQt4 import QtCore, QtGui, uic
+#from PyQt4.QtCore import pyqtSlot, pyqtSignal
+
+from cflib.crazyflie.log import LogVariable, LogConfig
+
+
+class LogConfigReader():
+ """Reads logging configurations from file"""
+
+ def __init__(self, crazyflie):
+ self.dsList = []
+ # Check if user config exists, otherwise copy files
+ if (not os.path.exists(sys.path[1] + "/log")):
+ logger.info("No user config found, copying dist files")
+ os.makedirs(sys.path[1] + "/log")
+ for f in glob.glob(sys.path[0] +
+ "/cfclient/configs/log/[A-Za-z]*.json"):
+ shutil.copy2(f, sys.path[1] + "/log")
+ self._cf = crazyflie
+ self._cf.connected.add_callback(self._connected)
+
+ def _read_config_files(self):
+ """Read and parse log configurations"""
+ configsfound = [os.path.basename(f) for f in
+ glob.glob(sys.path[1] + "/log/[A-Za-z_-]*.json")]
+ new_dsList = []
+ for conf in configsfound:
+ try:
+ logger.info("Parsing [%s]", conf)
+ json_data = open(sys.path[1] + "/log/%s" % conf)
+ self.data = json.load(json_data)
+ infoNode = self.data["logconfig"]["logblock"]
+
+ logConf = LogConfig(infoNode["name"],
+ int(infoNode["period"]))
+ for v in self.data["logconfig"]["logblock"]["variables"]:
+ if v["type"] == "TOC":
+ logConf.add_variable(str(v["name"]), v["fetch_as"])
+ else:
+ logConf.add_variable("Mem", v["fetch_as"],
+ v["stored_as"],
+ int(v["address"], 16))
+ new_dsList.append(logConf)
+ json_data.close()
+ except Exception as e:
+ logger.warning("Exception while parsing logconfig file: %s", e)
+ self.dsList = new_dsList
+
+ def _connected(self, link_uri):
+ """Callback that is called once Crazyflie is connected"""
+
+ self._read_config_files()
+ # Just add all the configurations. Via callbacks other parts of the
+ # application will pick up these configurations and use them
+ for d in self.dsList:
+ try:
+ self._cf.log.add_config(d)
+ except KeyError as e:
+ logger.warning(str(e))
+ except AttributeError as e:
+ logger.warning(str(e))
+
+ def getLogConfigs(self):
+ """Return the log configurations"""
+ return self.dsList
+
+ def saveLogConfigFile(self, logconfig):
+ """Save a log configuration to file"""
+ filename = sys.path[1] + "/log/" + logconfig.name + ".json"
+ logger.info("Saving config for [%s]", filename)
+
+ # Build tree for JSON
+ saveConfig = {}
+ logconf = {'logblock': {'variables': []}}
+ logconf['logblock']['name'] = logconfig.name
+ logconf['logblock']['period'] = logconfig.period_in_ms
+ # Temporary until plot is fixed
+
+ for v in logconfig.variables:
+ newC = {}
+ newC['name'] = v.name
+ newC['stored_as'] = v.stored_as_string
+ newC['fetch_as'] = v.fetch_as_string
+ newC['type'] = "TOC"
+ logconf['logblock']['variables'].append(newC)
+
+ saveConfig['logconfig'] = logconf
+
+ json_data = open(filename, 'w')
+ json_data.write(json.dumps(saveConfig, indent=2))
+ json_data.close()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..5bc212757895ae145bd54f853d5f53b9df89c06d
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logconfigreader.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.py
new file mode 100755
index 0000000000000000000000000000000000000000..d9c551c6c66da2779a74140953685399efe34c77
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.py
@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Used to write log data to files.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LogWriter']
+
+import os
+import sys
+import datetime
+
+import logging
+
+logger = logging.getLogger(__name__)
+
+from cflib.crazyflie.log import LogConfig
+
+import traceback
+
+
+class LogWriter():
+ """Create a writer for a specific log block"""
+
+ def __init__(self, logblock, connected_ts=None, directory=None):
+ """Initialize the writer"""
+ self._block = logblock
+ self._dir = directory
+ self._connected_ts = connected_ts
+
+ self._dir = os.path.join(sys.path[1], "logdata",
+ connected_ts.strftime("%Y%m%dT%H-%M-%S"))
+ self._file = None
+ self._header_written = False
+ self._header_values = []
+ self._filename = None
+
+ def _write_header(self):
+ """Write the header to the file"""
+ if not self._header_written:
+ s = "Timestamp"
+ for v in self._block.variables:
+ s += "," + v.name
+ self._header_values.append(v.name)
+ s += '\n'
+ self._file.write(s)
+ self._header_written = True
+
+ def _new_data(self, timestamp, data, logconf):
+ """Callback when new data arrives from the Crazyflie"""
+ if self._file:
+ s = "%d" % timestamp
+ for col in self._header_values:
+ s += "," + str(data[col])
+ s += '\n'
+ self._file.write(s)
+
+ def writing(self):
+ """Return True if the file is open and we are using it,
+ otherwise false"""
+ return True if self._file else False
+
+ def stop(self):
+ """Stop the logging to file"""
+ if self._file:
+ self._file.close()
+ self._file = None
+ self._block.data_received_cb.remove_callback(self._new_data)
+ logger.info("Stopped logging of block [%s] to file [%s]",
+ self._block.name, self._filename)
+ self._header_values = []
+
+ def start(self):
+ """Start the logging to file"""
+
+ # Due to concurrency let's not check first, just create
+ try:
+ os.makedirs(self._dir)
+ except OSError:
+ logger.debug("logdata directory already exists")
+
+ if not self._file:
+ time_now = datetime.datetime.now()
+ name = "{0}-{1}.csv".format(self._block.name,
+ time_now.strftime(
+ "%Y%m%dT%H-%M-%S"))
+ self._filename = os.path.join(self._dir, name)
+ self._file = open(self._filename, 'w')
+ self._write_header()
+ self._block.data_received_cb.add_callback(self._new_data)
+ logger.info("Started logging of block [%s] to file [%s]",
+ self._block.name, self._filename)
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..d41df77b73f2fa48831abd2082f3b08482223575
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/logdatawriter.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.py
new file mode 100755
index 0000000000000000000000000000000000000000..cfdff4ee84d8616cefa0928c9ea595b9a806c70e
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.py
@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013-2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+Implementation of a periodic timer that will call a callback every time
+the timer expires once started.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PeriodicTimer']
+
+import logging
+from threading import Thread
+from cflib.utils.callbacks import Caller
+import time
+
+logger = logging.getLogger(__name__)
+
+
+class PeriodicTimer:
+ """Create a periodic timer that will periodically call a callback"""
+
+ def __init__(self, period, callback):
+ self._callbacks = Caller()
+ self._callbacks.add_callback(callback)
+ self._started = False
+ self._period = period
+ self._thread = None
+
+ def start(self):
+ """Start the timer"""
+ if self._thread:
+ logger.warning("Timer already started, not restarting")
+ return
+ self._thread = _PeriodicTimerThread(self._period, self._callbacks)
+ self._thread.setDaemon(True)
+ self._thread.start()
+
+ def stop(self):
+ """Stop the timer"""
+ if self._thread:
+ self._thread.stop()
+ self._thread = None
+
+
+class _PeriodicTimerThread(Thread):
+ def __init__(self, period, caller):
+ super(_PeriodicTimerThread, self).__init__()
+ self._period = period
+ self._callbacks = caller
+ self._stop = False
+
+ def stop(self):
+ self._stop = True
+
+ def run(self):
+ while not self._stop:
+ time.sleep(self._period)
+ if self._stop:
+ break
+ self._callbacks.call()
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..04a5eb1373f92282b172bf825bba163593671ac3
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/periodictimer.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/pysdl2reader.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/pysdl2reader.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..371ba770c345e6c3e06779065a005f1d301ca081
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/pysdl2reader.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.py
new file mode 100755
index 0000000000000000000000000000000000000000..3a0dab0e343401d38227f58946aaab590abdfc56
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Singleton class.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Singleton']
+
+class Singleton(type):
+ """Class for making singletons"""
+ _instances = {}
+
+ def __call__(cls, *args, **kwargs):
+ """Called when creating new class"""
+ if cls not in cls._instances:
+ cls._instances[cls] = super(Singleton, cls).__call__(*args, **kwargs)
+ return cls._instances[cls]
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..ec89e13b651a261d597761710075f458596fbfb2
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/singleton.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.py
new file mode 100755
index 0000000000000000000000000000000000000000..8232b6c3ea1c050caa4a33d9112c1b3ae8f1d7d0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.py
@@ -0,0 +1,99 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2015 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
+# USA.
+
+"""
+Give access to the LED driver memory via ZMQ.
+"""
+
+import cflib
+import cflib.crazyflie
+from cflib.crazyflie.mem import MemoryElement
+
+import logging
+from threading import Thread, Lock
+ZMQ_PULL_PORT = 1024 + 190
+logger = logging.getLogger(__name__)
+
+enabled = False
+try:
+ import zmq
+ enabled = True
+except Exception as e:
+ logger.warning("Not enabling ZMQ LED driver access,"
+ "import failed ({})".format(e))
+
+class _PullReader(Thread):
+ """Blocking thread for reading from ZMQ socket"""
+ def __init__(self, receiver, callback, *args):
+ """Initialize"""
+ super(_PullReader, self).__init__(*args)
+ self._receiver = receiver
+ self._cb = callback
+ self.daemon = True
+ self.lock = Lock()
+
+ def run(self):
+ while True:
+ #self.lock.acquire()
+ self._cb(self._receiver.recv_json())
+
+
+class ZMQLEDDriver:
+ """Used for reading data from input devices using the PyGame API."""
+ def __init__(self, crazyflie):
+
+ if enabled:
+ self._cf = crazyflie
+ context = zmq.Context()
+ self._receiver = context.socket(zmq.PULL)
+ self._bind_addr = "tcp://*:{}".format(ZMQ_PULL_PORT)
+ # If the port is already bound an exception will be thrown
+ # and caught in the initialization of the readers and handled.
+ self._receiver.bind(self._bind_addr)
+ logger.info("Biding ZMQ for LED driver"
+ "at {}".format(self._bind_addr))
+ self._receiver_thread = _PullReader(self._receiver,
+ self._cmd_callback)
+
+ def start(self):
+ if enabled:
+ self._receiver_thread.start()
+
+ def _cmd_callback(self, data):
+ """Called when new data arrives via ZMQ"""
+ if len(self._cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)) > 0:
+ logger.info("Updating memory")
+ memory = self._cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0]
+ for i_led in range(len(data["rgbleds"])):
+ memory.leds[i_led].set(data["rgbleds"][i_led][0],
+ data["rgbleds"][i_led][1],
+ data["rgbleds"][i_led][2])
+ memory.write_data(self._write_cb)
+
+ def _write_cb(self, mem, addr):
+ return
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..9d11f15fe5930ef3137ec1223755222a21c76613
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_led_driver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.py b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.py
new file mode 100755
index 0000000000000000000000000000000000000000..e2ea3fc0738e380fc69ce191bfc8404b44c214e6
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2015 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
+# USA.
+
+"""
+Give access to the parameter framework via ZMQ.
+"""
+
+import logging
+from threading import Thread, Lock
+ZMQ_PULL_PORT = 1024 + 189
+logger = logging.getLogger(__name__)
+
+enabled = False
+try:
+ import zmq
+ enabled = True
+except Exception as e:
+ logger.warning("Not enabling ZMQ param access, import failed ({})".format(e))
+
+
+class _PullReader(Thread):
+
+ def __init__(self, receiver, callback, *args):
+ super(_PullReader, self).__init__(*args)
+ self._receiver = receiver
+ self._cb = callback
+ self.daemon = True
+ self.lock = Lock()
+
+ def run(self):
+ while True:
+ self.lock.acquire()
+ self._cb(self._receiver.recv_json())
+
+
+class ZMQParamAccess:
+ """Used for reading data from input devices using the PyGame API."""
+ def __init__(self, crazyflie):
+
+ if enabled:
+ self._cf = crazyflie
+ context = zmq.Context()
+ self._receiver = context.socket(zmq.REP)
+ self._bind_addr = "tcp://*:{}".format(ZMQ_PULL_PORT)
+ # If the port is already bound an exception will be thrown
+ # and caught in the initialization of the readers and handled.
+ self._receiver.bind(self._bind_addr)
+ logger.info("Biding ZMQ for parameters at {}".format(self._bind_addr))
+ self._receiver_thread = _PullReader(self._receiver, self._cmd_callback)
+
+ def start(self):
+ if enabled:
+ self._receiver_thread.start()
+
+ def _cmd_callback(self, data):
+ #logger.info(data)
+ if data["cmd"] == "toc":
+ response = {"version":1, "toc": []}
+ self._receiver.send_json(response)
+ self._receiver_thread.lock.release()
+ if data["cmd"] == "set":
+ resp = {"version": 1}
+ group = data["name"].split(".")[0]
+ name = data["name"].split(".")[1]
+ self._cf.param.add_update_callback(group=group, name=name,
+ cb=self._param_callback)
+ self._cf.param.set_value(data["name"], str(data["value"]))
+
+ def _param_callback(self, name, value):
+ group = name.split(".")[0]
+ name_short = name.split(".")[1]
+ logger.info("Removing {}.{}".format(group, name_short))
+ self._cf.param.remove_update_callback(group=group, name=name_short,
+ cb=self._param_callback)
+
+ response = {"version":1, "cmd": "set", "name": name, "value": value}
+ self._receiver.send_json(response)
+ self._receiver_thread.lock.release()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.pyc b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..168cdd01bcb4d437b887c1e9a61e7e9a3cf25da2
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cfclient/utils/zmq_param.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfconfig/Makefile b/pps_ws/src/d_fall_pps/crazyradio/cfconfig/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..78cdd24f88c97d0a226d51a7aea6245d2b600b32
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfconfig/Makefile
@@ -0,0 +1,51 @@
+
+######### JTAG and environment configuration ##########
+OPENOCD_INTERFACE ?= interface/jtagkey.cfg
+OPENOCD_TARGET ?= target/stm32.cfg
+#######################################################
+
+CONFIGFILE := cblock.cfg
+BINFILE := cblock.bin
+
+help:
+ @echo "Crazyflie config block makefile utility"
+ @echo
+ @echo "This script permits to read and write the config block of Crazyflie using"
+ @echo "openocd and any JTAG probe supported by it. The copter has to be connected"
+ @echo "to the jtag probe"
+ @echo
+ @echo "Available action:"
+ @echo " help : Display this message"
+ @echo " read : Read the configuration block from the copter using openOCD,"
+ @echo " decode it and save it in a configuration file"
+ @echo " write : Compile the configuration file to a block and write the block"
+ @echo " in the copter using openOCD"
+ @echo
+ @echo "Parameters that can be configured: [current value] (can be altered from command line)"
+ @echo " CONFIGFILE [$(CONFIGFILE)]"
+ @echo " BINFILE [$(BINFILE)]"
+ @echo " OCD_INTERFACE [$(OPENOCD_INTERFACE)]"
+ @echo " OCD_TARGET [$(OPENOCD_TARGET)]"
+
+read: readbin bin2cfg
+
+write: cfg2bin writebin
+
+readbin:
+ openocd -d0 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets \
+ -c "reset halt" -c "dump_image $(BINFILE) 0x1FC00 1024" \
+ -c "reset run" -c shutdown
+
+writebin:
+ openocd -d0 -f $(OPENOCD_INTERFACE) -f $(OPENOCD_TARGET) -c init -c targets \
+ -c "reset halt" -c "flash erase_sector 0 127 127" \
+ -c "flash write_bank 0 $(BINFILE) 0x1FC00" -c "verify_image $(BINFILE) 0x1FC00" \
+ -c "reset run" -c shutdown
+
+bin2cfg:
+ ./configblock.py extract $(BINFILE) $(CONFIGFILE)
+
+cfg2bin:
+ ./configblock.py generate $(CONFIGFILE) $(BINFILE)
+
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfconfig/configblock.py b/pps_ws/src/d_fall_pps/crazyradio/cfconfig/configblock.py
new file mode 100755
index 0000000000000000000000000000000000000000..c7a150fc12c197ca2f42430a516d2c6660242723
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfconfig/configblock.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Configuration block compiler/decompiler
+# Fully static version (to be considered as a prototype...)!
+from ConfigParser import ConfigParser
+import struct
+import sys
+
+# Radio speed enum type
+speeds = ["250K", "1M", "2M"]
+radioSpeedPos = 2
+
+defaultConfig = """#Crazyflie config block
+#default version generated from squatch
+[radio]
+channel= 100
+speed= 2M
+
+[calib]
+pitchTrim= 0.0
+rollTrim= 0.0
+"""
+
+config = """#Crazyflie config block
+#Block version %d extracted from copter
+[radio]
+channel= %d
+speed= %s
+
+[calib]
+pitchTrim= %f
+rollTrim= %f
+"""
+
+configVersion = 0
+structFormat = "<BBBff"
+
+
+def checksum256(st):
+ return reduce(lambda x, y: x + y, map(ord, st)) % 256
+
+
+def compileBlock(configFile, binFile):
+ config = ConfigParser()
+
+ config.read(configFile)
+
+ block = (configVersion,)
+
+ block += (config.getint("radio", "channel"),)
+ block += (speeds.index(config.get("radio", "speed").upper()),)
+
+ block += (config.getfloat("calib", "pitchTrim"),)
+ block += (config.getfloat("calib", "rollTrim"),)
+
+ bin = struct.pack(structFormat, *block)
+
+ # Adding some magic:
+ bin = "0xBC" + bin
+
+ bin += struct.pack("B", 256 - checksum256(bin))
+
+ # print("Config block checksum: %02x" % bin[len(bin)-1])
+
+ bfile = open(binFile, "w")
+ bfile.write(bin)
+ bfile.close()
+
+ print "Config block compiled successfully to", binFile
+
+
+def decompileBlock(binFile, configFile):
+ bfile = open(binFile)
+ bin = bfile.read()
+ bfile.close()
+
+ if (bin[0:4] != "0xBC" or
+ len(bin) < (struct.calcsize(structFormat) + 5) or
+ checksum256(bin[0:struct.calcsize(structFormat) + 5]) != 0):
+ print "Config block erased of altered, generating default file"
+ cfile = open(configFile, "w")
+ cfile.write(defaultConfig)
+ cfile.close()
+ else:
+ block = struct.unpack(structFormat,
+ bin[4:struct.calcsize(structFormat) + 4])
+ if block[0] != configVersion:
+ print("Error! wrong configuration block version, "
+ "this program must certainly be updated!")
+ return
+
+ block = (block[0:radioSpeedPos] + (speeds[block[radioSpeedPos]],) +
+ block[radioSpeedPos + 1:len(block)])
+
+ cfile = open(configFile, "w")
+ cfile.write(config % block)
+ cfile.close()
+ print "Config block successfully extracted to", configFile
+
+if __name__ == "__main__":
+ if (len(sys.argv) < 4 or
+ (sys.argv[1] != "generate" and sys.argv[1] != "extract")):
+ print "Configuration block compiler/decompiler."
+ print " Usage: %s <generate|extract> <infile> <outfile>" % sys.argv[0]
+
+ if sys.argv[1] == "generate":
+ compileBlock(sys.argv[2], sys.argv[3])
+ else:
+ decompileBlock(sys.argv[2], sys.argv[3])
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfheadless.py b/pps_ws/src/d_fall_pps/crazyradio/cfheadless.py
new file mode 100755
index 0000000000000000000000000000000000000000..cdfe003dc94df1531168a972c36a8fdb20ec4afb
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfheadless.py
@@ -0,0 +1,170 @@
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Headless client for the Crazyflie.
+"""
+
+import sys
+import os
+import logging
+import signal
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+import cfclient.utils
+from cfclient.utils.input import JoystickReader
+from cfclient.utils.config import Config
+
+if os.name == 'posix':
+ print 'Disabling standard output for libraries!'
+ stdout = os.dup(1)
+ os.dup2(os.open('/dev/null', os.O_WRONLY), 1)
+ sys.stdout = os.fdopen(stdout, 'w')
+
+# set SDL to use the dummy NULL video driver,
+# so it doesn't need a windowing system.
+os.environ["SDL_VIDEODRIVER"] = "dummy"
+
+class HeadlessClient():
+ """Crazyflie headless client"""
+
+ def __init__(self):
+ """Initialize the headless client and libraries"""
+ cflib.crtp.init_drivers()
+
+ self._jr = JoystickReader(do_device_discovery=False)
+
+ self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
+ rw_cache=sys.path[1]+"/cache")
+
+ signal.signal(signal.SIGINT, signal.SIG_DFL)
+
+ self._devs = []
+
+ for d in self._jr.available_devices():
+ self._devs.append(d.name)
+
+ def setup_controller(self, input_config, input_device=0, xmode=False):
+ """Set up the device reader"""
+ # Set up the joystick reader
+ self._jr.device_error.add_callback(self._input_dev_error)
+ print "Client side X-mode: %s" % xmode
+ if (xmode):
+ self._cf.commander.set_client_xmode(xmode)
+
+ devs = self._jr.available_devices()
+ print "Will use [%s] for input" % self._devs[input_device]
+ self._jr.start_input(self._devs[input_device])
+ self._jr.set_input_map(self._devs[input_device], input_config)
+
+ def controller_connected(self):
+ """ Return True if a controller is connected"""
+ return True if (len(self._jr.available_devices()) > 0) else False
+
+ def list_controllers(self):
+ """List the available controllers and input mapping"""
+ print "\nAvailable controllers:"
+ for i, dev in enumerate(self._devs):
+ print " - Controller #{}: {}".format(i, dev)
+ print "\nAvailable input mapping:"
+ for map in os.listdir(sys.path[1] + '/input'):
+ print " - " + map.split(".json")[0]
+
+ def connect_crazyflie(self, link_uri):
+ """Connect to a Crazyflie on the given link uri"""
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ # 2014-11-25 chad: Add a callback for when we have a good connection.
+ self._cf.connected.add_callback(self._connected)
+ self._cf.param.add_update_callback(group="imu_sensors", name="HMC5883L",
+ cb=(lambda name, found:
+ self._jr.set_alt_hold_available(eval(found))))
+ self._jr.althold_updated.add_callback(
+ lambda enabled: self._cf.param.set_value("flightmode.althold", enabled))
+
+ self._cf.open_link(link_uri)
+ self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
+
+ def _connected(self, link):
+ """Callback for a successful Crazyflie connection."""
+ print "Connected to {}".format(link)
+
+ def _connection_failed(self, link, message):
+ """Callback for a failed Crazyflie connection"""
+ print "Connection failed on {}: {}".format(link, message)
+ sys.exit(-1)
+
+ def _input_dev_error(self, message):
+ """Callback for an input device error"""
+ print "Error when reading device: {}".format(message)
+ sys.exit(-1)
+
+def main():
+ """Main Crazyflie headless application"""
+ import argparse
+
+ parser = argparse.ArgumentParser(prog="cfheadless")
+ parser.add_argument("-u", "--uri", action="store", dest="uri", type=str,
+ default="radio://0/10/250K",
+ help="URI to use for connection to the Crazyradio"
+ " dongle, defaults to radio://0/10/250K")
+ parser.add_argument("-i", "--input", action="store", dest="input",
+ type=str, default="PS3_Mode_1",
+ help="Input mapping to use for the controller,"
+ "defaults to PS3_Mode_1")
+ parser.add_argument("-d", "--debug", action="store_true", dest="debug",
+ help="Enable debug output")
+ parser.add_argument("-c", "--controller", action="store", type=int,
+ dest="controller", default=0,
+ help="Use controller with specified id,"
+ " id defaults to 0")
+ parser.add_argument("--controllers", action="store_true",
+ dest="list_controllers",
+ help="Only display available controllers and exit")
+ parser.add_argument("-x", "--x-mode", action="store_true",
+ dest="xmode",
+ help="Enable client-side X-mode")
+ (args, unused) = parser.parse_known_args()
+
+ if args.debug:
+ logging.basicConfig(level=logging.DEBUG)
+ else:
+ logging.basicConfig(level=logging.INFO)
+
+ headless = HeadlessClient()
+
+ if (args.list_controllers):
+ headless.list_controllers()
+ else:
+ if headless.controller_connected():
+ headless.setup_controller(input_config=args.input,
+ input_device=args.controller,
+ xmode=args.xmode)
+ headless.connect_crazyflie(link_uri=args.uri)
+ else:
+ print "No input-device connected, exiting!"
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..536acc1111695aade66f938f03689f8a10d71d4c
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.py
@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+The Crazyflie Micro Quadcopter library API used to communicate with the
+Crazyflie Micro Quadcopter via a communication link.
+
+The API takes care of scanning, opening and closing the communication link
+as well as sending/receiving data from the Crazyflie.
+
+A link is described using an URI of the following format:
+ <interface>://<interface defined data>.
+See each link for the data that can be included in the URI for that interface.
+
+The two main uses-cases are scanning for Crazyflies available on a
+communication link and opening a communication link to a Crazyflie.
+
+Example of scanning for available Crazyflies on all communication links:
+cflib.crtp.init_drivers()
+available = cflib.crtp.scan_interfaces()
+for i in available:
+ print "Found Crazyflie on URI [%s] with comment [%s]"
+ % (available[0], available[1])
+
+Example of connecting to a Crazyflie with know URI (radio dongle 0 and
+radio channel 125):
+cf = Crazyflie()
+cf.open_link("radio://0/125")
+...
+cf.close_link()
+"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c691ab2630d8b6d69d40d6d126cd82676449f86c
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..607735bb94569fc9e8d882a5b68d972894400cb7
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.py
@@ -0,0 +1,332 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Bootloading utilities for the Crazyflie.
+"""
+
+from cflib.utils.callbacks import Caller
+from .cloader import Cloader
+from .boottypes import BootVersion, TargetTypes, Target
+import zipfile
+import json
+import sys
+import time
+import logging
+logger = logging.getLogger(__name__)
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Bootloader']
+
+class Bootloader:
+
+ """Bootloader utility for the Crazyflie"""
+ def __init__(self, clink=None):
+ """Init the communication class by starting to communicate with the
+ link given. clink is the link address used after resetting to the
+ bootloader.
+
+ The device is actually considered in firmware mode.
+ """
+ self.clink = clink
+ self.in_loader = False
+
+ self.page_size = 0
+ self.buffer_pages = 0
+ self.flash_pages = 0
+ self.start_page = 0
+ self.cpuid = "N/A"
+ self.error_code = 0
+ self.protocol_version = 0
+
+ # Outgoing callbacks for progress
+ # int
+ self.progress_cb = None
+ # msg
+ self.error_cb = None
+ # bool
+ self.in_bootloader_cb = None
+ # Target
+ self.dev_info_cb = None
+
+ #self.dev_info_cb.add_callback(self._dev_info)
+ #self.in_bootloader_cb.add_callback(self._bootloader_info)
+
+ self._boot_plat = None
+
+ self._cload = Cloader(clink,
+ info_cb=self.dev_info_cb,
+ in_boot_cb=self.in_bootloader_cb)
+
+ def start_bootloader(self, warm_boot=False):
+ if warm_boot:
+ self._cload.open_bootloader_uri(self.clink)
+ started = self._cload.reset_to_bootloader(TargetTypes.NRF51)
+ if started:
+ started = self._cload.check_link_and_get_info()
+ else:
+ uri = self._cload.scan_for_bootloader()
+
+ # Workaround for libusb on Windows (open/close too fast)
+ time.sleep(1)
+
+ if uri:
+ self._cload.open_bootloader_uri(uri)
+ started = self._cload.check_link_and_get_info()
+ else:
+ started = False
+
+ if started:
+ self.protocol_version = self._cload.protocol_version
+
+ if self.protocol_version == BootVersion.CF1_PROTO_VER_0 or\
+ self.protocol_version == BootVersion.CF1_PROTO_VER_1:
+ # Nothing more to do
+ pass
+ elif self.protocol_version == BootVersion.CF2_PROTO_VER:
+ self._cload.request_info_update(TargetTypes.NRF51)
+ else:
+ print "Bootloader protocol 0x{:X} not supported!".self.protocol_version
+
+ return started
+
+ def get_target(self, target_id):
+ return self._cload.request_info_update(target_id)
+
+ def read_cf1_config(self):
+ """Read a flash page from the specified target"""
+ target = self._cload.targets[0xFF]
+ config_page = target.flash_pages - 1
+
+ return self._cload.read_flash(addr=0xFF, page=config_page)
+
+ def write_cf1_config(self, data):
+ target = self._cload.targets[0xFF]
+ config_page = target.flash_pages - 1
+
+ to_flash = {"target": target, "data": data, "type": "CF1 config",
+ "start_page": config_page}
+
+ self._internal_flash(target=to_flash)
+
+ def flash(self, filename, targets):
+ for target in targets:
+ if TargetTypes.from_string(target) not in self._cload.targets:
+ print "Target {} not found by bootloader".format(target)
+ return False
+
+ files_to_flash = ()
+ if zipfile.is_zipfile(filename):
+ # Read the manifest (don't forget to check so there is one!)
+ try:
+ zf = zipfile.ZipFile(filename)
+ j = json.loads(zf.read("manifest.json"))
+ files = j["files"]
+ if len(targets) == 0:
+ # No targets specified, just flash everything
+ for file in files:
+ if files[file]["target"] in targets:
+ targets[files[file]["target"]] += (files[file]["type"], )
+ else:
+ targets[files[file]["target"]] = (files[file]["type"], )
+
+ zip_targets = {}
+ for file in files:
+ file_name = file
+ file_info = files[file]
+ if file_info["target"] in zip_targets:
+ zip_targets[file_info["target"]][file_info["type"]] = {"filename": file_name}
+ else:
+ zip_targets[file_info["target"]] = {}
+ zip_targets[file_info["target"]][file_info["type"]] = {"filename": file_name}
+ except KeyError as e:
+ print e
+ print "No manifest.json in {}".format(filename)
+ return
+
+ try:
+ # Match and create targets
+ for target in targets:
+ t = targets[target]
+ for type in t:
+ file_to_flash = {}
+ current_target = "{}-{}".format(target, type)
+ file_to_flash["type"] = type
+ # Read the data, if this fails we bail
+ file_to_flash["target"] = self._cload.targets[TargetTypes.from_string(target)]
+ file_to_flash["data"] = zf.read(zip_targets[target][type]["filename"])
+ file_to_flash["start_page"] = file_to_flash["target"].start_page
+ files_to_flash += (file_to_flash, )
+ except KeyError as e:
+ print "Could not find a file for {} in {}".format(current_target, filename)
+ return False
+
+ else:
+ if len(targets) != 1:
+ print "Not an archive, must supply one target to flash"
+ else:
+ file_to_flash = {}
+ file_to_flash["type"] = "binary"
+ f = open(filename, 'rb')
+ for t in targets:
+ file_to_flash["target"] = self._cload.targets[TargetTypes.from_string(t)]
+ file_to_flash["type"] = targets[t][0]
+ file_to_flash["start_page"] = file_to_flash["target"].start_page
+ file_to_flash["data"] = f.read()
+ f.close()
+ files_to_flash += (file_to_flash, )
+
+ if not self.progress_cb:
+ print ""
+
+ file_counter = 0
+ for target in files_to_flash:
+ file_counter += 1
+ self._internal_flash(target, file_counter, len(files_to_flash))
+
+ def reset_to_firmware(self):
+ if self._cload.protocol_version == BootVersion.CF2_PROTO_VER:
+ self._cload.reset_to_firmware(TargetTypes.NRF51)
+ else:
+ self._cload.reset_to_firmware(TargetTypes.STM32)
+
+ def close(self):
+ if self._cload:
+ self._cload.close()
+
+ def _internal_flash(self, target, current_file_number=1, total_files=1):
+
+ image = target["data"]
+ t_data = target["target"]
+
+ start_page = target["start_page"]
+
+ # If used from a UI we need some extra things for reporting progress
+ factor = (100.0 * t_data.page_size) / len(image)
+ progress = 0
+
+ if self.progress_cb:
+ self.progress_cb("({}/{}) Starting...".format(current_file_number, total_files), int(progress))
+ else:
+ sys.stdout.write("Flashing {} of {} to {} ({}): ".format(current_file_number,
+ total_files,
+ TargetTypes.to_string(t_data.id),
+ target["type"]))
+ sys.stdout.flush()
+
+ if len(image) > ((t_data.flash_pages - start_page) *
+ t_data.page_size):
+ if self.progress_cb:
+ self.progress_cb("Error: Not enough space to flash the image file.", int(progress))
+ else:
+ print "Error: Not enough space to flash the image file."
+ raise Exception()
+
+ if not self.progress_cb:
+ logger.info(("%d bytes (%d pages) " % ((len(image) - 1),
+ int(len(image) / t_data.page_size) + 1)))
+ sys.stdout.write(("%d bytes (%d pages) " % ((len(image) - 1),
+ int(len(image) / t_data.page_size) + 1)))
+ sys.stdout.flush()
+
+ #For each page
+ ctr = 0 # Buffer counter
+ for i in range(0, int((len(image) - 1) / t_data.page_size) + 1):
+ #Load the buffer
+ if ((i + 1) * t_data.page_size) > len(image):
+ self._cload.upload_buffer(t_data.addr, ctr, 0, image[i * t_data.page_size:])
+ else:
+ self._cload.upload_buffer(t_data.addr, ctr, 0, image[i * t_data.page_size:
+ (i + 1) * t_data.page_size])
+
+ ctr += 1
+
+ if self.progress_cb:
+ progress += factor
+ self.progress_cb("({}/{}) Uploading buffer to {}...".format(current_file_number,
+ total_files,
+ TargetTypes.to_string(t_data.id)),
+
+ int(progress))
+ else:
+ sys.stdout.write(".")
+ sys.stdout.flush()
+
+ #Flash when the complete buffers are full
+ if ctr >= t_data.buffer_pages:
+ if self.progress_cb:
+ self.progress_cb("({}/{}) Writing buffer to {}...".format(current_file_number,
+ total_files,
+ TargetTypes.to_string(t_data.id)),
+
+ int(progress))
+ else:
+ sys.stdout.write("%d" % ctr)
+ sys.stdout.flush()
+ if not self._cload.write_flash(t_data.addr, 0,
+ start_page + i - (ctr - 1),
+ ctr):
+ if self.progress_cb:
+ self.progress_cb("Error during flash operation (code %d)".format(self._cload.error_code),
+ int(progress))
+ else:
+ print "\nError during flash operation (code %d). Maybe"\
+ " wrong radio link?" % self._cload.error_code
+ raise Exception()
+
+ ctr = 0
+
+ if ctr > 0:
+ if self.progress_cb:
+ self.progress_cb("({}/{}) Writing buffer to {}...".format(current_file_number,
+ total_files,
+ TargetTypes.to_string(t_data.id)),
+ int(progress))
+ else:
+ sys.stdout.write("%d" % ctr)
+ sys.stdout.flush()
+ if not self._cload.write_flash(t_data.addr,
+ 0,
+ (start_page +
+ (int((len(image) - 1) / t_data.page_size)) -
+ (ctr - 1)),
+ ctr):
+ if self.progress_cb:
+ self.progress_cb("Error during flash operation (code %d)".format(self._cload.error_code),
+ int(progress))
+ else:
+ print "\nError during flash operation (code %d). Maybe"\
+ " wrong radio link?" % self._cload.error_code
+ raise Exception()
+
+
+ if self.progress_cb:
+ self.progress_cb("({}/{}) Flashing done!".format(current_file_number, total_files),
+ int(100))
+ else:
+ print ""
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..7a9308cd970b3857545387746bf010b61da7192f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.py
new file mode 100755
index 0000000000000000000000000000000000000000..03d5755720ea6517948cfb3d23c61ed5d595de43
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Bootloading utilities for the Crazyflie.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['BootVersion', 'TargetTypes', 'Target']
+
+class BootVersion:
+ CF1_PROTO_VER_0 = 0x00
+ CF1_PROTO_VER_1 = 0x01
+ CF2_PROTO_VER = 0x10
+
+ @staticmethod
+ def to_ver_string(ver):
+ if ver == BootVersion.CF1_PROTO_VER_0 or ver == BootVersion.CF1_PROTO_VER_1:
+ return "Crazyflie Nano Quadcopter (1.0)"
+ if ver == BootVersion.CF2_PROTO_VER:
+ return "Crazyflie 2.0"
+ return "Unknown"
+
+class TargetTypes:
+ STM32 = 0xFF
+ NRF51 = 0xFE
+
+ @staticmethod
+ def to_string(target):
+ if target == TargetTypes.STM32:
+ return "stm32"
+ if target == TargetTypes.NRF51:
+ return "nrf51"
+ return "Unknown"
+
+ @staticmethod
+ def from_string(name):
+ if name == "stm32":
+ return TargetTypes.STM32
+ if name == "nrf51":
+ return TargetTypes.NRF51
+ return 0
+
+class Target:
+
+ def __init__(self, id):
+ self.id = id
+ self.protocol_version = 0xFF
+ self.page_size = 0
+ self.buffer_pages = 0
+ self.flash_pages = 0
+ self.start_page = 0
+ self.cpuid = ""
+ self.data = None
+
+ def __str__(self):
+ ret = ""
+ ret += "Target info: {} (0x{:X})\n".format(TargetTypes.to_string(self.id), self.id)
+ ret += "Flash pages: %d | Page size: %d | Buffer pages: %d |"\
+ " Start page: %d\n" % (self.flash_pages, self.page_size,
+ self.buffer_pages, self.start_page)
+ ret += "%d KBytes of flash available for firmware image." % (
+ (self.flash_pages - self.start_page) * self.page_size / 1024)
+ return ret
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..90d465f1d2cbc96f07d096ca87420d253950c703
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.py
new file mode 100755
index 0000000000000000000000000000000000000000..47a108cf568b57b26b7701f7e71df61fa3e21338
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.py
@@ -0,0 +1,420 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Crazyflie radio bootloader for flashing firmware.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Cloader']
+
+import logging
+logger = logging.getLogger(__name__)
+
+import time
+import struct
+import math
+import random
+
+import cflib.crtp
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+
+from .boottypes import TargetTypes, Target
+
+
+class Cloader:
+ """Bootloader utility for the Crazyflie"""
+ def __init__(self, link, info_cb=None, in_boot_cb=None):
+ """Init the communication class by starting to communicate with the
+ link given. clink is the link address used after resetting to the
+ bootloader.
+
+ The device is actually considered in firmware mode.
+ """
+ self.link = None
+ self.uri = link
+ self.in_loader = False
+
+ self.page_size = 0
+ self.buffer_pages = 0
+ self.flash_pages = 0
+ self.start_page = 0
+ self.cpuid = "N/A"
+ self.error_code = 0
+ self.protocol_version = 0xFF
+
+ self._info_cb = info_cb
+ self._in_boot_cb = in_boot_cb
+
+ self.targets = {}
+ self.mapping = None
+ self._available_boot_uri = ("radio://0/110/2M", "radio://0/0/2M")
+
+ def close(self):
+ """ Close the link """
+ if self.link:
+ self.link.close()
+
+ def scan_for_bootloader(self):
+ link = cflib.crtp.get_link_driver("radio://0")
+ ts = time.time()
+ res = ()
+ while len(res) == 0 and (time.time() - ts) < 10:
+ res = link.scan_selected(self._available_boot_uri)
+
+ link.close()
+
+ if len(res) > 0:
+ return res[0]
+ return None
+
+ def reset_to_bootloader(self, target_id):
+ retry_counter = 5
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = (target_id, 0xFF)
+ self.link.send_packet(pk)
+
+ pk = self.link.receive_packet(1)
+
+ while ((not pk or pk.header != 0xFF or
+ struct.unpack("<BB", pk.data[0:2]) != (target_id, 0xFF)) and
+ retry_counter >= 0 ):
+
+ pk = self.link.receive_packet(1)
+ retry_counter -= 1
+
+ if pk:
+ new_address = (0xb1, ) + struct.unpack("<BBBB", pk.data[2:6][::-1])
+
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = (target_id, 0xF0, 0x00)
+ self.link.send_packet(pk)
+
+ addr = int(struct.pack("B"*5, *new_address).encode('hex'), 16)
+
+ time.sleep(0.2)
+ self.link.close()
+ time.sleep(0.2)
+ self.link = cflib.crtp.get_link_driver("radio://0/0/2M/{}".format(addr))
+
+ return True
+ else:
+ return False
+
+
+ def reset_to_bootloader1(self, cpu_id):
+ """ Reset to the bootloader
+ The parameter cpuid shall correspond to the device to reset.
+
+ Return true if the reset has been done and the contact with the
+ bootloader is established.
+ """
+ #Send an echo request and wait for the answer
+ #Mainly aim to bypass a bug of the crazyflie firmware that prevents
+ #reset before normal CRTP communication
+ pk = CRTPPacket()
+ pk.port = CRTPPort.LINKCTRL
+ pk.data = (1, 2, 3) + cpu_id
+ self.link.send_packet(pk)
+
+ pk = None
+ while True:
+ pk = self.link.receive_packet(2)
+ if not pk:
+ return False
+
+ if pk.port == CRTPPort.LINKCTRL:
+ break
+
+ #Send the reset to bootloader request
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = (0xFF, 0xFE) + cpu_id
+ self.link.send_packet(pk)
+
+ #Wait to ack the reset ...
+ pk = None
+ while True:
+ pk = self.link.receive_packet(2)
+ if not pk:
+ return False
+
+ if pk.port == 0xFF and tuple(pk.data) == (0xFF, 0xFE) + cpu_id:
+ pk.data = (0xFF, 0xF0) + cpu_id
+ self.link.send_packet(pk)
+ break
+
+ time.sleep(0.1)
+ self.link.close()
+ self.link = cflib.crtp.get_link_driver(self.clink_address)
+ #time.sleep(0.1)
+
+ return self._update_info()
+
+ def reset_to_firmware(self, target_id):
+ """ Reset to firmware
+ The parameter cpuid shall correspond to the device to reset.
+
+ Return true if the reset has been done
+ """
+ # The fake CPU ID is legacy from the Crazyflie 1.0
+ # In order to reset the CPU id had to be sent, but this
+ # was removed before launching it. But the length check is
+ # still in the bootloader. So to work around this bug so
+ # some extra data needs to be sent.
+ fake_cpu_id = (1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12)
+ #Send the reset to bootloader request
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = (target_id, 0xFF) + fake_cpu_id
+ self.link.send_packet(pk)
+
+ #Wait to ack the reset ...
+ pk = None
+ while True:
+ pk = self.link.receive_packet(2)
+ if not pk:
+ return False
+
+ if (pk.header == 0xFF and
+ struct.unpack("B" * len(pk.data),
+ pk.data)[:2] == (target_id, 0xFF)):
+ # Difference in CF1 and CF2 (CPU ID)
+ if target_id == 0xFE:
+ pk.data = (target_id, 0xF0, 0x01)
+ else:
+ pk.data = (target_id, 0xF0) + fake_cpu_id
+ self.link.send_packet(pk)
+ break
+
+ time.sleep(0.1)
+
+ def open_bootloader_uri(self, uri=None):
+ if self.link:
+ self.link.close()
+ if uri:
+ self.link = cflib.crtp.get_link_driver(uri)
+ else:
+ self.link = cflib.crtp.get_link_driver(self.clink_address)
+
+ def check_link_and_get_info(self, target_id=0xFF):
+ """Try to get a connection with the bootloader by requesting info
+ 5 times. This let roughly 10 seconds to boot the copter ..."""
+ for _ in range(0, 5):
+ if self._update_info(target_id):
+ if self._in_boot_cb:
+ self._in_boot_cb.call(True, self.targets[target_id].protocol_version)
+ if self._info_cb:
+ self._info_cb.call(self.targets[target_id])
+ if self.protocol_version != 1:
+ return True
+ # Set radio link to a random address
+ addr = [0xbc] + map(lambda x: random.randint(0, 255), range(4))
+ return self._set_address(addr)
+ return False
+
+ def _set_address(self, new_address):
+ """ Change copter radio address.
+ This function works only with crazyradio CRTP link.
+ """
+
+ logging.debug("Setting bootloader radio address to"
+ " {}".format(new_address))
+
+ if len(new_address) != 5:
+ raise Exception("Radio address should be 5 bytes long")
+
+ self.link.pause()
+
+ for _ in range(10):
+ logging.debug("Trying to set new radio address")
+ self.link.cradio.set_address((0xE7,) * 5)
+ pkdata = (0xFF, 0xFF, 0x11) + tuple(new_address)
+ self.link.cradio.send_packet(pkdata)
+ self.link.cradio.set_address(tuple(new_address))
+ if self.link.cradio.send_packet((0xff,)).ack:
+ logging.info("Bootloader set to radio address"
+ " {}".format(new_address))
+ self.link.restart()
+ return True
+
+ self.link.restart()
+ return False
+
+ def request_info_update(self, target_id):
+ if target_id not in self.targets:
+ self._update_info(target_id)
+ if self._info_cb:
+ self._info_cb.call(self.targets[target_id])
+ return self.targets[target_id]
+
+ def _update_info(self, target_id):
+ """ Call the command getInfo and fill up the information received in
+ the fields of the object
+ """
+
+ #Call getInfo ...
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = (target_id, 0x10)
+ self.link.send_packet(pk)
+
+ #Wait for the answer
+ pk = self.link.receive_packet(2)
+
+ if (pk and pk.header == 0xFF and
+ struct.unpack("<BB", pk.data[0:2]) == (target_id, 0x10)):
+ tab = struct.unpack("BBHHHH", pk.data[0:10])
+ cpuid = struct.unpack("B" * 12, pk.data[10:22])
+ if not target_id in self.targets:
+ self.targets[target_id] = Target(target_id)
+ self.targets[target_id].addr = target_id
+ if len(pk.data) > 22:
+ self.targets[target_id].protocol_version = pk.datat[22]
+ self.protocol_version = pk.datat[22]
+ self.targets[target_id].page_size = tab[2]
+ self.targets[target_id].buffer_pages = tab[3]
+ self.targets[target_id].flash_pages = tab[4]
+ self.targets[target_id].start_page = tab[5]
+ self.targets[target_id].cpuid = "%02X" % cpuid[0]
+ for i in cpuid[1:]:
+ self.targets[target_id].cpuid += ":%02X" % i
+
+ if self.protocol_version == 0x10 and target_id == TargetTypes.STM32:
+ self._update_mapping(target_id)
+
+ return True
+
+ return False
+
+ def _update_mapping(self, target_id):
+ pk = CRTPPacket()
+ pk.set_header(0xff, 0xff)
+ pk.data = (target_id, 0x12)
+ self.link.send_packet(pk)
+
+ pk = self.link.receive_packet(2)
+
+ if (pk and pk.header == 0xFF and
+ struct.unpack("<BB", pk.data[0:2]) == (target_id, 0x12)):
+ m = pk.datat[2:]
+
+ if (len(m)%2)!=0:
+ raise Exception("Malformed flash mapping packet")
+
+ self.mapping = []
+ page = 0
+ for i in range(len(m)/2):
+ for j in range(m[2*i]):
+ self.mapping.append(page)
+ page += m[(2*i)+1]
+
+ def upload_buffer(self, target_id, page, address, buff):
+ """Upload data into a buffer on the Crazyflie"""
+ #print len(buff)
+ count = 0
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = struct.pack("=BBHH", target_id, 0x14, page, address)
+
+ for i in range(0, len(buff)):
+ pk.data += buff[i]
+
+ count += 1
+
+ if count > 24:
+ self.link.send_packet(pk)
+ count = 0
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = struct.pack("=BBHH", target_id, 0x14, page,
+ i + address + 1)
+
+ self.link.send_packet(pk)
+
+ def read_flash(self, addr=0xFF, page=0x00):
+ """Read back a flash page from the Crazyflie and return it"""
+ buff = ""
+
+ page_size = self.targets[addr].page_size
+
+ for i in range(0, int(math.ceil(page_size / 25.0))):
+ pk = None
+ retry_counter = 5
+ while ((not pk or pk.header != 0xFF or
+ struct.unpack("<BB", pk.data[0:2]) != (addr, 0x1C))
+ and retry_counter >= 0):
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = struct.pack("<BBHH", addr, 0x1C, page, (i * 25))
+ self.link.send_packet(pk)
+
+ pk = self.link.receive_packet(1)
+ retry_counter -= 1
+ if (retry_counter < 0):
+ return None
+ else:
+ buff += pk.data[6:]
+
+ return buff[0:page_size] # For some reason we get one byte extra here...
+
+ def write_flash(self, addr, page_buffer, target_page, page_count):
+ """Initiate flashing of data in the buffer to flash."""
+ #print "Write page", flashPage
+ #print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
+ pk = None
+ retry_counter = 5
+ #print "Flasing to 0x{:X}".format(addr)
+ while ((not pk or pk.header != 0xFF or
+ struct.unpack("<BB", pk.data[0:2]) != (addr, 0x18))
+ and retry_counter >= 0):
+ pk = CRTPPacket()
+ pk.set_header(0xFF, 0xFF)
+ pk.data = struct.pack("<BBHHH", addr, 0x18, page_buffer,
+ target_page, page_count)
+ self.link.send_packet(pk)
+ pk = self.link.receive_packet(1)
+ retry_counter -= 1
+
+ if retry_counter < 0:
+ self.error_code = -1
+ return False
+
+ self.error_code = ord(pk.data[3])
+
+ return ord(pk.data[2]) == 1
+
+ def decode_cpu_id(self, cpuid):
+ """Decode the CPU id into a string"""
+ ret = ()
+ for i in cpuid.split(':'):
+ ret += (eval("0x" + i), )
+
+ return ret
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..2e08af236fe20c0ef672a99b002bf7af340b8cea
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/27A2C4BA.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/27A2C4BA.json
new file mode 100755
index 0000000000000000000000000000000000000000..b71d9c655880540900b4577694ccadb68934003a
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/27A2C4BA.json
@@ -0,0 +1,257 @@
+{
+ "sensorfusion6": {
+ "ki": {
+ "ident": 24,
+ "group": "sensorfusion6",
+ "name": "ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kp": {
+ "ident": 23,
+ "group": "sensorfusion6",
+ "name": "kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "cpu": {
+ "flash": {
+ "ident": 1,
+ "group": "cpu",
+ "name": "flash",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "id2": {
+ "ident": 4,
+ "group": "cpu",
+ "name": "id2",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 0
+ },
+ "id0": {
+ "ident": 2,
+ "group": "cpu",
+ "name": "id0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 0
+ },
+ "id1": {
+ "ident": 3,
+ "group": "cpu",
+ "name": "id1",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 0
+ }
+ },
+ "version": {
+ "revision0": {
+ "ident": 25,
+ "group": "version",
+ "name": "revision0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 0
+ },
+ "revision1": {
+ "ident": 26,
+ "group": "version",
+ "name": "revision1",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ }
+ },
+ "pid_rate": {
+ "yaw_kp": {
+ "ident": 11,
+ "group": "pid_rate",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 7,
+ "group": "pid_rate",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 8,
+ "group": "pid_rate",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 6,
+ "group": "pid_rate",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 9,
+ "group": "pid_rate",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 5,
+ "group": "pid_rate",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 13,
+ "group": "pid_rate",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 12,
+ "group": "pid_rate",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 10,
+ "group": "pid_rate",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "pid_attitude": {
+ "yaw_kp": {
+ "ident": 20,
+ "group": "pid_attitude",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 16,
+ "group": "pid_attitude",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 17,
+ "group": "pid_attitude",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 15,
+ "group": "pid_attitude",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 18,
+ "group": "pid_attitude",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 14,
+ "group": "pid_attitude",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 22,
+ "group": "pid_attitude",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 21,
+ "group": "pid_attitude",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 19,
+ "group": "pid_attitude",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "imu_acc_lpf": {
+ "factor": {
+ "ident": 0,
+ "group": "imu_acc_lpf",
+ "name": "factor",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/81204C68.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/81204C68.json
new file mode 100755
index 0000000000000000000000000000000000000000..bf0d26da7b36bb2d494b68b27975266918273b8d
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/81204C68.json
@@ -0,0 +1,317 @@
+{
+ "acc": {
+ "y": {
+ "ident": 26,
+ "group": "acc",
+ "name": "y",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "x": {
+ "ident": 25,
+ "group": "acc",
+ "name": "x",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "z": {
+ "ident": 27,
+ "group": "acc",
+ "name": "z",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "zw": {
+ "ident": 28,
+ "group": "acc",
+ "name": "zw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "gyro": {
+ "y": {
+ "ident": 4,
+ "group": "gyro",
+ "name": "y",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "x": {
+ "ident": 3,
+ "group": "gyro",
+ "name": "x",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "z": {
+ "ident": 5,
+ "group": "gyro",
+ "name": "z",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "sys": {
+ "canfly": {
+ "ident": 2,
+ "group": "sys",
+ "name": "canfly",
+ "pytype": "<b",
+ "__class__": "LogTocElement",
+ "ctype": "int8_t",
+ "access": 0
+ }
+ },
+ "stabilizer": {
+ "thrust": {
+ "ident": 9,
+ "group": "stabilizer",
+ "name": "thrust",
+ "pytype": "<H",
+ "__class__": "LogTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "yaw": {
+ "ident": 8,
+ "group": "stabilizer",
+ "name": "yaw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll": {
+ "ident": 6,
+ "group": "stabilizer",
+ "name": "roll",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch": {
+ "ident": 7,
+ "group": "stabilizer",
+ "name": "pitch",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "motor": {
+ "m4": {
+ "ident": 21,
+ "group": "motor",
+ "name": "m4",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m1": {
+ "ident": 22,
+ "group": "motor",
+ "name": "m1",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m3": {
+ "ident": 24,
+ "group": "motor",
+ "name": "m3",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m2": {
+ "ident": 23,
+ "group": "motor",
+ "name": "m2",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ }
+ },
+ "altHold": {
+ "vSpeed": {
+ "ident": 18,
+ "group": "altHold",
+ "name": "vSpeed",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "target": {
+ "ident": 16,
+ "group": "altHold",
+ "name": "target",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "err": {
+ "ident": 15,
+ "group": "altHold",
+ "name": "err",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASL": {
+ "ident": 19,
+ "group": "altHold",
+ "name": "vSpeedASL",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedAcc": {
+ "ident": 20,
+ "group": "altHold",
+ "name": "vSpeedAcc",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "zSpeed": {
+ "ident": 17,
+ "group": "altHold",
+ "name": "zSpeed",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "vpid": {
+ "i": {
+ "ident": 31,
+ "group": "vpid",
+ "name": "i",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "p": {
+ "ident": 30,
+ "group": "vpid",
+ "name": "p",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pid": {
+ "ident": 29,
+ "group": "vpid",
+ "name": "pid",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "d": {
+ "ident": 32,
+ "group": "vpid",
+ "name": "d",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "baro": {
+ "aslRaw": {
+ "ident": 11,
+ "group": "baro",
+ "name": "aslRaw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslLong": {
+ "ident": 12,
+ "group": "baro",
+ "name": "aslLong",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pressure": {
+ "ident": 14,
+ "group": "baro",
+ "name": "pressure",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "temp": {
+ "ident": 13,
+ "group": "baro",
+ "name": "temp",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "asl": {
+ "ident": 10,
+ "group": "baro",
+ "name": "asl",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "pm": {
+ "state": {
+ "ident": 1,
+ "group": "pm",
+ "name": "state",
+ "pytype": "<b",
+ "__class__": "LogTocElement",
+ "ctype": "int8_t",
+ "access": 0
+ },
+ "vbat": {
+ "ident": 0,
+ "group": "pm",
+ "name": "vbat",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/892049D2.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/892049D2.json
new file mode 100755
index 0000000000000000000000000000000000000000..7de8289371fc71c20c9c28d0b3fe5e0ecc9d3945
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/892049D2.json
@@ -0,0 +1,89 @@
+{
+ "stabilizer": {
+ "thrust": {
+ "ident": 4,
+ "group": "stabilizer",
+ "name": "thrust",
+ "pytype": "<H",
+ "__class__": "LogTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "yaw": {
+ "ident": 3,
+ "group": "stabilizer",
+ "name": "yaw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll": {
+ "ident": 1,
+ "group": "stabilizer",
+ "name": "roll",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch": {
+ "ident": 2,
+ "group": "stabilizer",
+ "name": "pitch",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "motor": {
+ "m4": {
+ "ident": 5,
+ "group": "motor",
+ "name": "m4",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m1": {
+ "ident": 6,
+ "group": "motor",
+ "name": "m1",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m3": {
+ "ident": 8,
+ "group": "motor",
+ "name": "m3",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m2": {
+ "ident": 7,
+ "group": "motor",
+ "name": "m2",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ }
+ },
+ "pm": {
+ "vbat": {
+ "ident": 0,
+ "group": "pm",
+ "name": "vbat",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/9E5F2E7D.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/9E5F2E7D.json
new file mode 100755
index 0000000000000000000000000000000000000000..9c5467ac90ae5bfcce962e7c5fd82976199a1634
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/9E5F2E7D.json
@@ -0,0 +1,355 @@
+{
+ "acc": {
+ "y": {
+ "ident": 4,
+ "group": "acc",
+ "name": "y",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "x": {
+ "ident": 3,
+ "group": "acc",
+ "name": "x",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "z": {
+ "ident": 5,
+ "group": "acc",
+ "name": "z",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "zw": {
+ "ident": 6,
+ "group": "acc",
+ "name": "zw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "mag2": {
+ "ident": 7,
+ "group": "acc",
+ "name": "mag2",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "mag": {
+ "y": {
+ "ident": 35,
+ "group": "mag",
+ "name": "y",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "x": {
+ "ident": 34,
+ "group": "mag",
+ "name": "x",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "z": {
+ "ident": 36,
+ "group": "mag",
+ "name": "z",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "gyro": {
+ "y": {
+ "ident": 28,
+ "group": "gyro",
+ "name": "y",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "x": {
+ "ident": 27,
+ "group": "gyro",
+ "name": "x",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "z": {
+ "ident": 29,
+ "group": "gyro",
+ "name": "z",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "sys": {
+ "canfly": {
+ "ident": 2,
+ "group": "sys",
+ "name": "canfly",
+ "pytype": "<b",
+ "__class__": "LogTocElement",
+ "ctype": "int8_t",
+ "access": 0
+ }
+ },
+ "stabilizer": {
+ "thrust": {
+ "ident": 20,
+ "group": "stabilizer",
+ "name": "thrust",
+ "pytype": "<H",
+ "__class__": "LogTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "yaw": {
+ "ident": 19,
+ "group": "stabilizer",
+ "name": "yaw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll": {
+ "ident": 17,
+ "group": "stabilizer",
+ "name": "roll",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch": {
+ "ident": 18,
+ "group": "stabilizer",
+ "name": "pitch",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "motor": {
+ "m4": {
+ "ident": 13,
+ "group": "motor",
+ "name": "m4",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m1": {
+ "ident": 14,
+ "group": "motor",
+ "name": "m1",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m3": {
+ "ident": 16,
+ "group": "motor",
+ "name": "m3",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ },
+ "m2": {
+ "ident": 15,
+ "group": "motor",
+ "name": "m2",
+ "pytype": "<i",
+ "__class__": "LogTocElement",
+ "ctype": "int32_t",
+ "access": 0
+ }
+ },
+ "altHold": {
+ "vSpeed": {
+ "ident": 24,
+ "group": "altHold",
+ "name": "vSpeed",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "target": {
+ "ident": 22,
+ "group": "altHold",
+ "name": "target",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "err": {
+ "ident": 21,
+ "group": "altHold",
+ "name": "err",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASL": {
+ "ident": 25,
+ "group": "altHold",
+ "name": "vSpeedASL",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedAcc": {
+ "ident": 26,
+ "group": "altHold",
+ "name": "vSpeedAcc",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "zSpeed": {
+ "ident": 23,
+ "group": "altHold",
+ "name": "zSpeed",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "vpid": {
+ "i": {
+ "ident": 32,
+ "group": "vpid",
+ "name": "i",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "p": {
+ "ident": 31,
+ "group": "vpid",
+ "name": "p",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pid": {
+ "ident": 30,
+ "group": "vpid",
+ "name": "pid",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "d": {
+ "ident": 33,
+ "group": "vpid",
+ "name": "d",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "baro": {
+ "aslRaw": {
+ "ident": 9,
+ "group": "baro",
+ "name": "aslRaw",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslLong": {
+ "ident": 10,
+ "group": "baro",
+ "name": "aslLong",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pressure": {
+ "ident": 12,
+ "group": "baro",
+ "name": "pressure",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "temp": {
+ "ident": 11,
+ "group": "baro",
+ "name": "temp",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "asl": {
+ "ident": 8,
+ "group": "baro",
+ "name": "asl",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "pm": {
+ "state": {
+ "ident": 1,
+ "group": "pm",
+ "name": "state",
+ "pytype": "<b",
+ "__class__": "LogTocElement",
+ "ctype": "int8_t",
+ "access": 0
+ },
+ "vbat": {
+ "ident": 0,
+ "group": "pm",
+ "name": "vbat",
+ "pytype": "<f",
+ "__class__": "LogTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E20076B8.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E20076B8.json
new file mode 100755
index 0000000000000000000000000000000000000000..ca46a4a32a276732106252199be7b54d2283d214
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E20076B8.json
@@ -0,0 +1,499 @@
+{
+ "imu_sensors": {
+ "HMC5883L": {
+ "ident": 3,
+ "group": "imu_sensors",
+ "name": "HMC5883L",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MS5611": {
+ "ident": 4,
+ "group": "imu_sensors",
+ "name": "MS5611",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "sensorfusion6": {
+ "ki": {
+ "ident": 30,
+ "group": "sensorfusion6",
+ "name": "ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kp": {
+ "ident": 29,
+ "group": "sensorfusion6",
+ "name": "kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "flightmode": {
+ "althold": {
+ "ident": 10,
+ "group": "flightmode",
+ "name": "althold",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 0
+ }
+ },
+ "firmware": {
+ "revision0": {
+ "ident": 50,
+ "group": "firmware",
+ "name": "revision0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "revision1": {
+ "ident": 51,
+ "group": "firmware",
+ "name": "revision1",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 1
+ },
+ "modified": {
+ "ident": 52,
+ "group": "firmware",
+ "name": "modified",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "cpu": {
+ "flash": {
+ "ident": 6,
+ "group": "cpu",
+ "name": "flash",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 1
+ },
+ "id2": {
+ "ident": 9,
+ "group": "cpu",
+ "name": "id2",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "id0": {
+ "ident": 7,
+ "group": "cpu",
+ "name": "id0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "id1": {
+ "ident": 8,
+ "group": "cpu",
+ "name": "id1",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ }
+ },
+ "pid_rate": {
+ "yaw_kp": {
+ "ident": 17,
+ "group": "pid_rate",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 13,
+ "group": "pid_rate",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 14,
+ "group": "pid_rate",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 12,
+ "group": "pid_rate",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 15,
+ "group": "pid_rate",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 11,
+ "group": "pid_rate",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 19,
+ "group": "pid_rate",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 18,
+ "group": "pid_rate",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 16,
+ "group": "pid_rate",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "pid_attitude": {
+ "yaw_kp": {
+ "ident": 26,
+ "group": "pid_attitude",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 22,
+ "group": "pid_attitude",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 23,
+ "group": "pid_attitude",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 21,
+ "group": "pid_attitude",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 24,
+ "group": "pid_attitude",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 20,
+ "group": "pid_attitude",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 28,
+ "group": "pid_attitude",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 27,
+ "group": "pid_attitude",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 25,
+ "group": "pid_attitude",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "altHold": {
+ "vSpeedAccFac": {
+ "ident": 43,
+ "group": "altHold",
+ "name": "vSpeedAccFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASLDeadband": {
+ "ident": 44,
+ "group": "altHold",
+ "name": "vSpeedASLDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "errDeadband": {
+ "ident": 33,
+ "group": "altHold",
+ "name": "errDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "altHoldErrMax": {
+ "ident": 35,
+ "group": "altHold",
+ "name": "altHoldErrMax",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kd": {
+ "ident": 36,
+ "group": "altHold",
+ "name": "kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pidAslFac": {
+ "ident": 40,
+ "group": "altHold",
+ "name": "pidAslFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "maxThrust": {
+ "ident": 48,
+ "group": "altHold",
+ "name": "maxThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "ki": {
+ "ident": 37,
+ "group": "altHold",
+ "name": "ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedLimit": {
+ "ident": 46,
+ "group": "altHold",
+ "name": "vSpeedLimit",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslAlpha": {
+ "ident": 31,
+ "group": "altHold",
+ "name": "aslAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kp": {
+ "ident": 38,
+ "group": "altHold",
+ "name": "kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "baseThrust": {
+ "ident": 47,
+ "group": "altHold",
+ "name": "baseThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "vBiasAlpha": {
+ "ident": 42,
+ "group": "altHold",
+ "name": "vBiasAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pidAlpha": {
+ "ident": 39,
+ "group": "altHold",
+ "name": "pidAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "altHoldChangeSens": {
+ "ident": 34,
+ "group": "altHold",
+ "name": "altHoldChangeSens",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "minThrust": {
+ "ident": 49,
+ "group": "altHold",
+ "name": "minThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "vAccDeadband": {
+ "ident": 41,
+ "group": "altHold",
+ "name": "vAccDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslAlphaLong": {
+ "ident": 32,
+ "group": "altHold",
+ "name": "aslAlphaLong",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASLFac": {
+ "ident": 45,
+ "group": "altHold",
+ "name": "vSpeedASLFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "imu_tests": {
+ "HMC5883L": {
+ "ident": 1,
+ "group": "imu_tests",
+ "name": "HMC5883L",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MS5611": {
+ "ident": 2,
+ "group": "imu_tests",
+ "name": "MS5611",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MPU6050": {
+ "ident": 0,
+ "group": "imu_tests",
+ "name": "MPU6050",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "imu_acc_lpf": {
+ "factor": {
+ "ident": 5,
+ "group": "imu_acc_lpf",
+ "name": "factor",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E8BC7DAD.json b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E8BC7DAD.json
new file mode 100755
index 0000000000000000000000000000000000000000..ca46a4a32a276732106252199be7b54d2283d214
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/cache/E8BC7DAD.json
@@ -0,0 +1,499 @@
+{
+ "imu_sensors": {
+ "HMC5883L": {
+ "ident": 3,
+ "group": "imu_sensors",
+ "name": "HMC5883L",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MS5611": {
+ "ident": 4,
+ "group": "imu_sensors",
+ "name": "MS5611",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "sensorfusion6": {
+ "ki": {
+ "ident": 30,
+ "group": "sensorfusion6",
+ "name": "ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kp": {
+ "ident": 29,
+ "group": "sensorfusion6",
+ "name": "kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "flightmode": {
+ "althold": {
+ "ident": 10,
+ "group": "flightmode",
+ "name": "althold",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 0
+ }
+ },
+ "firmware": {
+ "revision0": {
+ "ident": 50,
+ "group": "firmware",
+ "name": "revision0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "revision1": {
+ "ident": 51,
+ "group": "firmware",
+ "name": "revision1",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 1
+ },
+ "modified": {
+ "ident": 52,
+ "group": "firmware",
+ "name": "modified",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "cpu": {
+ "flash": {
+ "ident": 6,
+ "group": "cpu",
+ "name": "flash",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 1
+ },
+ "id2": {
+ "ident": 9,
+ "group": "cpu",
+ "name": "id2",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "id0": {
+ "ident": 7,
+ "group": "cpu",
+ "name": "id0",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ },
+ "id1": {
+ "ident": 8,
+ "group": "cpu",
+ "name": "id1",
+ "pytype": "<L",
+ "__class__": "ParamTocElement",
+ "ctype": "uint32_t",
+ "access": 1
+ }
+ },
+ "pid_rate": {
+ "yaw_kp": {
+ "ident": 17,
+ "group": "pid_rate",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 13,
+ "group": "pid_rate",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 14,
+ "group": "pid_rate",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 12,
+ "group": "pid_rate",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 15,
+ "group": "pid_rate",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 11,
+ "group": "pid_rate",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 19,
+ "group": "pid_rate",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 18,
+ "group": "pid_rate",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 16,
+ "group": "pid_rate",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "pid_attitude": {
+ "yaw_kp": {
+ "ident": 26,
+ "group": "pid_attitude",
+ "name": "yaw_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kd": {
+ "ident": 22,
+ "group": "pid_attitude",
+ "name": "roll_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kp": {
+ "ident": 23,
+ "group": "pid_attitude",
+ "name": "pitch_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_ki": {
+ "ident": 21,
+ "group": "pid_attitude",
+ "name": "roll_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_ki": {
+ "ident": 24,
+ "group": "pid_attitude",
+ "name": "pitch_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "roll_kp": {
+ "ident": 20,
+ "group": "pid_attitude",
+ "name": "roll_kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_kd": {
+ "ident": 28,
+ "group": "pid_attitude",
+ "name": "yaw_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "yaw_ki": {
+ "ident": 27,
+ "group": "pid_attitude",
+ "name": "yaw_ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pitch_kd": {
+ "ident": 25,
+ "group": "pid_attitude",
+ "name": "pitch_kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "altHold": {
+ "vSpeedAccFac": {
+ "ident": 43,
+ "group": "altHold",
+ "name": "vSpeedAccFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASLDeadband": {
+ "ident": 44,
+ "group": "altHold",
+ "name": "vSpeedASLDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "errDeadband": {
+ "ident": 33,
+ "group": "altHold",
+ "name": "errDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "altHoldErrMax": {
+ "ident": 35,
+ "group": "altHold",
+ "name": "altHoldErrMax",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kd": {
+ "ident": 36,
+ "group": "altHold",
+ "name": "kd",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pidAslFac": {
+ "ident": 40,
+ "group": "altHold",
+ "name": "pidAslFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "maxThrust": {
+ "ident": 48,
+ "group": "altHold",
+ "name": "maxThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "ki": {
+ "ident": 37,
+ "group": "altHold",
+ "name": "ki",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedLimit": {
+ "ident": 46,
+ "group": "altHold",
+ "name": "vSpeedLimit",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslAlpha": {
+ "ident": 31,
+ "group": "altHold",
+ "name": "aslAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "kp": {
+ "ident": 38,
+ "group": "altHold",
+ "name": "kp",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "baseThrust": {
+ "ident": 47,
+ "group": "altHold",
+ "name": "baseThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "vBiasAlpha": {
+ "ident": 42,
+ "group": "altHold",
+ "name": "vBiasAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "pidAlpha": {
+ "ident": 39,
+ "group": "altHold",
+ "name": "pidAlpha",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "altHoldChangeSens": {
+ "ident": 34,
+ "group": "altHold",
+ "name": "altHoldChangeSens",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "minThrust": {
+ "ident": 49,
+ "group": "altHold",
+ "name": "minThrust",
+ "pytype": "<H",
+ "__class__": "ParamTocElement",
+ "ctype": "uint16_t",
+ "access": 0
+ },
+ "vAccDeadband": {
+ "ident": 41,
+ "group": "altHold",
+ "name": "vAccDeadband",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "aslAlphaLong": {
+ "ident": 32,
+ "group": "altHold",
+ "name": "aslAlphaLong",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ },
+ "vSpeedASLFac": {
+ "ident": 45,
+ "group": "altHold",
+ "name": "vSpeedASLFac",
+ "pytype": "<f",
+ "__class__": "ParamTocElement",
+ "ctype": "float",
+ "access": 0
+ }
+ },
+ "imu_tests": {
+ "HMC5883L": {
+ "ident": 1,
+ "group": "imu_tests",
+ "name": "HMC5883L",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MS5611": {
+ "ident": 2,
+ "group": "imu_tests",
+ "name": "MS5611",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ },
+ "MPU6050": {
+ "ident": 0,
+ "group": "imu_tests",
+ "name": "MPU6050",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 1
+ }
+ },
+ "imu_acc_lpf": {
+ "factor": {
+ "ident": 5,
+ "group": "imu_acc_lpf",
+ "name": "factor",
+ "pytype": "<B",
+ "__class__": "ParamTocElement",
+ "ctype": "uint8_t",
+ "access": 0
+ }
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..0f7400da8a0e8d9977649098002983e506f04d15
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.py
@@ -0,0 +1,391 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+The Crazyflie module is used to easily connect/send/receive data
+from a Crazyflie.
+
+Each function in the Crazyflie has a class in the module that can be used
+to access that functionality. The same design is then used in the Crazyflie
+firmware which makes the mapping 1:1 in most cases.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Crazyflie']
+
+import logging
+logger = logging.getLogger(__name__)
+import time
+import datetime
+from threading import Thread
+
+from threading import Timer, Lock
+
+from .commander import Commander
+from .console import Console
+from .param import Param
+from .log import Log
+from .toccache import TocCache
+from .mem import Memory
+from .platformservice import PlatformService
+
+import cflib.crtp
+
+from cflib.utils.callbacks import Caller
+
+
+class State:
+ """Stat of the connection procedure"""
+ DISCONNECTED = 0
+ INITIALIZED = 1
+ CONNECTED = 2
+ SETUP_FINISHED = 3
+
+
+class Crazyflie():
+ """The Crazyflie class"""
+
+ # Called on disconnect, no matter the reason
+ disconnected = Caller()
+ # Called on unintentional disconnect only
+ connection_lost = Caller()
+ # Called when the first packet in a new link is received
+ link_established = Caller()
+ # Called when the user requests a connection
+ connection_requested = Caller()
+ # Called when the link is established and the TOCs (that are not cached)
+ # have been downloaded
+ connected = Caller()
+ # Called if establishing of the link fails (i.e times out)
+ connection_failed = Caller()
+ # Called for every packet received
+ packet_received = Caller()
+ # Called for every packet sent
+ packet_sent = Caller()
+ # Called when the link driver updates the link quality measurement
+ link_quality_updated = Caller()
+
+ state = State.DISCONNECTED
+
+ def __init__(self, link=None, ro_cache=None, rw_cache=None):
+ """
+ Create the objects from this module and register callbacks.
+
+ ro_cache -- Path to read-only cache (string)
+ rw_cache -- Path to read-write cache (string)
+ """
+ self.link = link
+ self._toc_cache = TocCache(ro_cache=ro_cache,
+ rw_cache=rw_cache)
+
+ self.incoming = _IncomingPacketHandler(self)
+ self.incoming.setDaemon(True)
+ self.incoming.start()
+
+ self.commander = Commander(self)
+ self.log = Log(self)
+ self.console = Console(self)
+ self.param = Param(self)
+ self.mem = Memory(self)
+ self.platform = PlatformService(self)
+
+ self.link_uri = ""
+
+ # Used for retry when no reply was sent back
+ self.packet_received.add_callback(self._check_for_initial_packet_cb)
+ self.packet_received.add_callback(self._check_for_answers)
+
+ self._answer_patterns = {}
+
+ self._send_lock = Lock()
+
+ self.connected_ts = None
+
+ # Connect callbacks to logger
+ self.disconnected.add_callback(
+ lambda uri: logger.info("Callback->Disconnected from [%s]", uri))
+ self.disconnected.add_callback(self._disconnected)
+ self.link_established.add_callback(
+ lambda uri: logger.info("Callback->Connected to [%s]", uri))
+ self.connection_lost.add_callback(
+ lambda uri, errmsg: logger.info("Callback->Connection lost to"
+ " [%s]: %s", uri, errmsg))
+ self.connection_failed.add_callback(
+ lambda uri, errmsg: logger.info("Callback->Connected failed to"
+ " [%s]: %s", uri, errmsg))
+ self.connection_requested.add_callback(
+ lambda uri: logger.info("Callback->Connection initialized[%s]", uri))
+ self.connected.add_callback(
+ lambda uri: logger.info("Callback->Connection setup finished [%s]", uri))
+
+ def _disconnected(self, link_uri):
+ """ Callback when disconnected."""
+ self.connected_ts = None
+
+ def _start_connection_setup(self):
+ """Start the connection setup by refreshing the TOCs"""
+ logger.info("We are connected[%s], request connection setup",
+ self.link_uri)
+ print "We are connected[%s], request connection setup" % self.link_uri
+ self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache)
+
+ def _param_toc_updated_cb(self):
+ """Called when the param TOC has been fully updated"""
+ logger.info("Param TOC finished updating")
+ self.connected_ts = datetime.datetime.now()
+ self.connected.call(self.link_uri)
+ # Trigger the update for all the parameters
+ self.param.request_update_of_all_params()
+
+ def _mems_updated_cb(self):
+ """Called when the memories have been identified"""
+ logger.info("Memories finished updating")
+ self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache)
+
+ def _log_toc_updated_cb(self):
+ """Called when the log TOC has been fully updated"""
+ logger.info("Log TOC finished updating")
+ self.mem.refresh(self._mems_updated_cb)
+
+ def _link_error_cb(self, errmsg):
+ """Called from the link driver when there's an error"""
+ logger.warning("Got link error callback [%s] in state [%s]",
+ errmsg, self.state)
+ if (self.link is not None):
+ self.link.close()
+ self.link = None
+ if (self.state == State.INITIALIZED):
+ self.connection_failed.call(self.link_uri, errmsg)
+ if (self.state == State.CONNECTED or
+ self.state == State.SETUP_FINISHED):
+ self.disconnected.call(self.link_uri)
+ self.connection_lost.call(self.link_uri, errmsg)
+ self.state = State.DISCONNECTED
+
+ def _link_quality_cb(self, percentage):
+ """Called from link driver to report link quality"""
+ self.link_quality_updated.call(percentage)
+
+ def _check_for_initial_packet_cb(self, data):
+ """
+ Called when first packet arrives from Crazyflie.
+
+ This is used to determine if we are connected to something that is
+ answering.
+ """
+ self.state = State.CONNECTED
+ self.link_established.call(self.link_uri)
+ self.packet_received.remove_callback(self._check_for_initial_packet_cb)
+
+ def open_link(self, link_uri):
+ """
+ Open the communication link to a copter at the given URI and setup the
+ connection (download log/parameter TOC).
+ """
+ self.connection_requested.call(link_uri)
+ self.state = State.INITIALIZED
+ self.link_uri = link_uri
+ try:
+ self.link = cflib.crtp.get_link_driver(link_uri,
+ self._link_quality_cb,
+ self._link_error_cb)
+
+ if not self.link:
+ message = "No driver found or malformed URI: {}"\
+ .format(link_uri)
+ print message
+ logger.warning(message)
+ self.connection_failed.call(link_uri, message)
+ else:
+ # Add a callback so we can check that any data is coming
+ # back from the copter
+ self.packet_received.add_callback(self._check_for_initial_packet_cb)
+ self._start_connection_setup()
+ print "connection setup started"
+
+ except Exception as ex: # pylint: disable=W0703
+ # We want to catch every possible exception here and show
+ # it in the user interface
+ import traceback
+ logger.error("Couldn't load link driver: %s\n\n%s",
+ ex, traceback.format_exc())
+ exception_text = "Couldn't load link driver: %s\n\n%s" % (
+ ex, traceback.format_exc())
+ print "connection probably failed"
+ print exception_text
+ if self.link:
+ self.link.close()
+ self.link = None
+ self.connection_failed.call(link_uri, exception_text)
+
+ def close_link(self):
+ """Close the communication link."""
+ logger.info("Closing link")
+ if (self.link is not None):
+ self.commander.send_setpoint(0, 0, 0, 0)
+ if (self.link is not None):
+ self.link.close()
+ self.link = None
+ self._answer_patterns = {}
+ self.disconnected.call(self.link_uri)
+
+ def add_port_callback(self, port, cb):
+ """Add a callback to cb on port"""
+ self.incoming.add_port_callback(port, cb)
+
+ def remove_port_callback(self, port, cb):
+ """Remove the callback cb on port"""
+ self.incoming.remove_port_callback(port, cb)
+
+ def _no_answer_do_retry(self, pk, pattern):
+ """Resend packets that we have not gotten answers to"""
+ logger.debug("Resending for pattern %s", pattern)
+ # Set the timer to None before trying to send again
+ self.send_packet(pk, expected_reply=pattern, resend=True)
+
+ def _check_for_answers(self, pk):
+ """
+ Callback called for every packet received to check if we are
+ waiting for an answer on this port. If so, then cancel the retry
+ timer.
+ """
+ longest_match = ()
+ if len(self._answer_patterns) > 0:
+ data = (pk.header,) + pk.datat
+ for p in self._answer_patterns.keys():
+ logger.debug("Looking for pattern match on %s vs %s", p, data)
+ if len(p) <= len(data):
+ if p == data[0:len(p)]:
+ match = data[0:len(p)]
+ if len(match) >= len(longest_match):
+ logger.debug("Found new longest match %s", match)
+ longest_match = match
+ if len(longest_match) > 0:
+ self._answer_patterns[longest_match].cancel()
+ del self._answer_patterns[longest_match]
+
+ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2):
+ """
+ Send a packet through the link interface.
+
+ pk -- Packet to send
+ expect_answer -- True if a packet from the Crazyflie is expected to
+ be sent back, otherwise false
+
+ """
+ self._send_lock.acquire()
+ if self.link is not None:
+ if len(expected_reply) > 0 and not resend and \
+ self.link.needs_resending:
+ pattern = (pk.header,) + expected_reply
+ logger.debug("Sending packet and expecting the %s pattern back",
+ pattern)
+ new_timer = Timer(timeout,
+ lambda: self._no_answer_do_retry(pk, pattern))
+ self._answer_patterns[pattern] = new_timer
+ new_timer.start()
+ elif resend:
+ # Check if we have gotten an answer, if not try again
+ pattern = expected_reply
+ if pattern in self._answer_patterns:
+ logger.debug("We want to resend and the pattern is there")
+ if self._answer_patterns[pattern]:
+ new_timer = Timer(timeout,
+ lambda:
+ self._no_answer_do_retry(
+ pk, pattern))
+ self._answer_patterns[pattern] = new_timer
+ new_timer.start()
+ else:
+ logger.debug("Resend requested, but no pattern found: %s",
+ self._answer_patterns)
+ self.link.send_packet(pk)
+ self.packet_sent.call(pk)
+ self._send_lock.release()
+
+class _IncomingPacketHandler(Thread):
+ """Handles incoming packets and sends the data to the correct receivers"""
+ def __init__(self, cf):
+ Thread.__init__(self)
+ self.cf = cf
+ self.cb = []
+
+ def add_port_callback(self, port, cb):
+ """Add a callback for data that comes on a specific port"""
+ logger.debug("Adding callback on port [%d] to [%s]", port, cb)
+ self.add_header_callback(cb, port, 0, 0xff, 0x0)
+
+ def remove_port_callback(self, port, cb):
+ """Remove a callback for data that comes on a specific port"""
+ logger.debug("Removing callback on port [%d] to [%s]", port, cb)
+ for port_callback in self.cb:
+ if (port_callback[0] == port and port_callback[4] == cb):
+ self.cb.remove(port_callback)
+
+ def add_header_callback(self, cb, port, channel, port_mask=0xFF,
+ channel_mask=0xFF):
+ """
+ Add a callback for a specific port/header callback with the
+ possibility to add a mask for channel and port for multiple
+ hits for same callback.
+ """
+ self.cb.append([port, port_mask, channel, channel_mask, cb])
+
+ def run(self):
+ while(True):
+ if self.cf.link is None:
+ time.sleep(1)
+ continue
+ pk = self.cf.link.receive_packet(1)
+
+ if pk is None:
+ continue
+
+ #All-packet callbacks
+ self.cf.packet_received.call(pk)
+
+ found = False
+ for cb in self.cb:
+ if (cb[0] == pk.port & cb[1] and
+ cb[2] == pk.channel & cb[3]):
+ try:
+ cb[4](pk)
+ except Exception: # pylint: disable=W0703
+ # Disregard pylint warning since we want to catch all
+ # exceptions and we can't know what will happen in
+ # the callbacks.
+ import traceback
+ logger.warning("Exception while doing callback on port"
+ " [%d]\n\n%s", pk.port,
+ traceback.format_exc())
+ if (cb[0] != 0xFF):
+ found = True
+
+ if not found:
+ logger.warning("Got packet on header (%d,%d) but no callback "
+ "to handle it", pk.port, pk.channel)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4ce7384997efaabbe7f55a3e6b14360cde89900a
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py
new file mode 100755
index 0000000000000000000000000000000000000000..6e723c82d7f5542871b0ed41bdae3d0eef45619e
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Used for sending control setpoints to the Crazyflie
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Commander']
+
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+import struct
+
+
+class Commander():
+ """
+ Used for sending control setpoints to the Crazyflie
+ """
+
+ def __init__(self, crazyflie=None):
+ """
+ Initialize the commander object. By default the commander is in
+ +-mode (not x-mode).
+ """
+ self._cf = crazyflie
+ self._x_mode = False
+
+ def set_client_xmode(self, enabled):
+ """
+ Enable/disable the client side X-mode. When enabled this recalculates
+ the setpoints before sending them to the Crazyflie.
+ """
+ self._x_mode = enabled
+
+ def send_setpoint(self, roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
+ """
+ Send a new control setpoint for roll/pitch/yaw/thrust to the copter
+
+ The arguments roll/pitch/yaw/trust is the new setpoints that should
+ be sent to the copter
+ """
+ """
+ if thrust > 0xFFFF or thrust < 0:
+ raise ValueError("Thrust must be between 0 and 0xFFFF")
+
+ if self._x_mode:
+ roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch)
+ """
+ pk = CRTPPacket()
+ pk.port = CRTPPort.COMMANDER
+ """ pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust) """
+ pk.data = struct.pack('<fffHHHHHB', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
+ self._cf.send_packet(pk)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..60e1d661e5f06bdbfeaf1bbe0d8449209016fa04
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.py
new file mode 100755
index 0000000000000000000000000000000000000000..bdc566582dbc551b2805d2b216e42687b17ff6f2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+"""
+Crazyflie console is used to receive characters printed using printf
+from the firmware.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Console']
+
+import struct
+from cflib.utils.callbacks import Caller
+from cflib.crtp.crtpstack import CRTPPort
+
+
+class Console:
+ """
+ Crazyflie console is used to receive characters printed using printf
+ from the firmware.
+ """
+
+ receivedChar = Caller()
+
+ def __init__(self, crazyflie):
+ """
+ Initialize the console and register it to receive data from the copter.
+ """
+ self.cf = crazyflie
+ self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming)
+
+ def incoming(self, packet):
+ """
+ Callback for data received from the copter.
+ """
+ # This might be done prettier ;-)
+ console_text = "%s" % struct.unpack("%is" % len(packet.data), packet.data)
+
+ self.receivedChar.call(console_text)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7dbd2d9d6d6da40aa68d74f41c7059d3ff08e269
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.py
new file mode 100755
index 0000000000000000000000000000000000000000..2f1ab2fd59f5da07f13ac30c2152199903acfb4f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.py
@@ -0,0 +1,539 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Enables logging of variables from the Crazyflie.
+
+When a Crazyflie is connected it's possible to download a TableOfContent of all
+the variables that can be logged. Using this it's possible to add logging
+configurations where selected variables are sent to the client at a
+specified period.
+
+Terminology:
+ Log configuration - A configuration with a period and a number of variables
+ that are present in the TOC.
+ Stored as - The size and type of the variable as declared in the
+ Crazyflie firmware
+ Fetch as - The size and type that a variable should be fetched as.
+ This does not have to be the same as the size and type
+ it's stored as.
+
+States of a configuration:
+ Created on host - When a configuration is created the contents is checked
+ so that all the variables are present in the TOC. If not
+ then the configuration cannot be created.
+ Created on CF - When the configuration is deemed valid it is added to the
+ Crazyflie. At this time the memory constraint is checked
+ and the status returned.
+ Started on CF - Any added block that is not started can be started. Once
+ started the Crazyflie will send back logdata periodically
+ according to the specified period when it's created.
+ Stopped on CF - Any started configuration can be stopped. The memory taken
+ by the configuration on the Crazyflie is NOT freed, the
+ only effect is that the Crazyflie will stop sending
+ logdata back to the host.
+ Deleted on CF - Any block that is added can be deleted. When this is done
+ the memory taken by the configuration is freed on the
+ Crazyflie. The configuration will have to be re-added to
+ be used again.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Log', 'LogTocElement']
+
+import struct
+import errno
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+from cflib.utils.callbacks import Caller
+from .toc import Toc, TocFetcher
+
+# Channels used for the logging port
+CHAN_TOC = 0
+CHAN_SETTINGS = 1
+CHAN_LOGDATA = 2
+
+# Commands used when accessing the Table of Contents
+CMD_TOC_ELEMENT = 0
+CMD_TOC_INFO = 1
+
+# Commands used when accessing the Log configurations
+CMD_CREATE_BLOCK = 0
+CMD_APPEND_BLOCK = 1
+CMD_DELETE_BLOCK = 2
+CMD_START_LOGGING = 3
+CMD_STOP_LOGGING = 4
+CMD_RESET_LOGGING = 5
+
+# Possible states when receiving TOC
+IDLE = "IDLE"
+GET_TOC_INF = "GET_TOC_INFO"
+GET_TOC_ELEMENT = "GET_TOC_ELEMENT"
+
+# The max size of a CRTP packet payload
+MAX_LOG_DATA_PACKET_SIZE = 30
+
+import logging
+logger = logging.getLogger(__name__)
+
+class LogVariable():
+ """A logging variable"""
+
+ TOC_TYPE = 0
+ MEM_TYPE = 1
+
+ def __init__(self, name="", fetchAs="uint8_t", varType=TOC_TYPE,
+ storedAs="", address=0):
+ self.name = name
+ self.fetch_as = LogTocElement.get_id_from_cstring(fetchAs)
+ if (len(storedAs) == 0):
+ self.stored_as = self.fetch_as
+ else:
+ self.stored_as = LogTocElement.get_id_from_cstring(storedAs)
+ self.address = address
+ self.type = varType
+ self.stored_as_string = storedAs
+ self.fetch_as_string = fetchAs
+
+ def is_toc_variable(self):
+ """
+ Return true if the variable should be in the TOC, false if raw memory
+ variable
+ """
+ return self.type == LogVariable.TOC_TYPE
+
+ def get_storage_and_fetch_byte(self):
+ """Return what the variable is stored as and fetched as"""
+ return (self.fetch_as | (self.stored_as << 4))
+
+ def __str__(self):
+ return ("LogVariable: name=%s, store=%s, fetch=%s" %
+ (self.name, LogTocElement.get_cstring_from_id(self.stored_as),
+ LogTocElement.get_cstring_from_id(self.fetch_as)))
+
+class LogConfig(object):
+ """Representation of one log configuration that enables logging
+ from the Crazyflie"""
+ _config_id_counter = 1
+
+ def __init__(self, name, period_in_ms):
+ """Initialize the entry"""
+ self.data_received_cb = Caller()
+ self.error_cb = Caller()
+ self.started_cb = Caller()
+ self.added_cb = Caller()
+ self.err_no = 0
+
+ self.id = LogConfig._config_id_counter
+ LogConfig._config_id_counter = (LogConfig._config_id_counter + 1) % 255
+ self.cf = None
+ self.period = period_in_ms / 10
+ self.period_in_ms = period_in_ms
+ self._added = False
+ self._started = False
+ self.valid = False
+ self.variables = []
+ self.default_fetch_as = []
+ self.name = name
+
+ def add_variable(self, name, fetch_as=None):
+ """Add a new variable to the configuration.
+
+ name - Complete name of the variable in the form group.name
+ fetch_as - String representation of the type the variable should be
+ fetched as (i.e uint8_t, float, FP16, etc)
+
+ If no fetch_as type is supplied, then the stored as type will be used
+ (i.e the type of the fetched variable is the same as it's stored in the
+ Crazyflie)."""
+ if fetch_as:
+ self.variables.append(LogVariable(name, fetch_as))
+ else:
+ # We cannot determine the default type until we have connected. So
+ # save the name and we will add these once we are connected.
+ self.default_fetch_as.append(name)
+
+ def add_memory(self, name, fetch_as, stored_as, address):
+ """Add a raw memory position to log.
+
+ name - Arbitrary name of the variable
+ fetch_as - String representation of the type of the data the memory
+ should be fetch as (i.e uint8_t, float, FP16)
+ stored_as - String representation of the type the data is stored as
+ in the Crazyflie
+ address - The address of the data
+ """
+ self.variables.append(LogVariable(name, fetch_as, LogVariable.MEM_TYPE,
+ stored_as, address))
+
+ def _set_added(self, added):
+ if added != self._added:
+ self.added_cb.call(self, added)
+ self._added = added
+
+ def _get_added(self):
+ return self._added
+
+ def _set_started(self, started):
+ if started != self._started:
+ self.started_cb.call(self, started)
+ self._started = started
+
+ def _get_started(self):
+ return self._started
+
+ added = property(_get_added, _set_added)
+ started = property(_get_started, _set_started)
+
+ def create(self):
+ """Save the log configuration in the Crazyflie"""
+ pk = CRTPPacket()
+ pk.set_header(5, CHAN_SETTINGS)
+ pk.data = (CMD_CREATE_BLOCK, self.id)
+ for var in self.variables:
+ if (var.is_toc_variable() is False): # Memory location
+ logger.debug("Logging to raw memory %d, 0x%04X",
+ var.get_storage_and_fetch_byte(), var.address)
+ pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
+ pk.data += struct.pack('<I', var.address)
+ else: # Item in TOC
+ logger.debug("Adding %s with id=%d and type=0x%02X",
+ var.name,
+ self.cf.log.toc.get_element_id(
+ var.name), var.get_storage_and_fetch_byte())
+ pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
+ pk.data += struct.pack('<B', self.cf.log.toc.
+ get_element_id(var.name))
+ logger.debug("Adding log block id {}".format(self.id))
+ self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
+
+ def start(self):
+ """Start the logging for this entry"""
+ if (self.cf.link is not None):
+ if (self._added is False):
+ self.create()
+ logger.debug("First time block is started, add block")
+ else:
+ logger.debug("Block already registered, starting logging"
+ " for id=%d", self.id)
+ pk = CRTPPacket()
+ pk.set_header(5, CHAN_SETTINGS)
+ pk.data = (CMD_START_LOGGING, self.id, self.period)
+ self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id))
+
+ def stop(self):
+ """Stop the logging for this entry"""
+ if (self.cf.link is not None):
+ if (self.id is None):
+ logger.warning("Stopping block, but no block registered")
+ else:
+ logger.debug("Sending stop logging for block id=%d", self.id)
+ pk = CRTPPacket()
+ pk.set_header(5, CHAN_SETTINGS)
+ pk.data = (CMD_STOP_LOGGING, self.id)
+ self.cf.send_packet(pk, expected_reply=(CMD_STOP_LOGGING, self.id))
+
+ def delete(self):
+ """Delete this entry in the Crazyflie"""
+ if (self.cf.link is not None):
+ if (self.id is None):
+ logger.warning("Delete block, but no block registered")
+ else:
+ logger.debug("LogEntry: Sending delete logging for block id=%d"
+ % self.id)
+ pk = CRTPPacket()
+ pk.set_header(5, CHAN_SETTINGS)
+ pk.data = (CMD_DELETE_BLOCK, self.id)
+ self.cf.send_packet(pk, expected_reply=(CMD_DELETE_BLOCK, self.id))
+
+ def unpack_log_data(self, log_data, timestamp):
+ """Unpack received logging data so it represent real values according
+ to the configuration in the entry"""
+ ret_data = {}
+ data_index = 0
+ for var in self.variables:
+ size = LogTocElement.get_size_from_id(var.fetch_as)
+ name = var.name
+ unpackstring = LogTocElement.get_unpack_string_from_id(
+ var.fetch_as)
+ value = struct.unpack(unpackstring,
+ log_data[data_index:data_index + size])[0]
+ data_index += size
+ ret_data[name] = value
+ self.data_received_cb.call(timestamp, ret_data, self)
+
+
+class LogTocElement:
+ """An element in the Log TOC."""
+ types = {0x01: ("uint8_t", '<B', 1),
+ 0x02: ("uint16_t", '<H', 2),
+ 0x03: ("uint32_t", '<L', 4),
+ 0x04: ("int8_t", '<b', 1),
+ 0x05: ("int16_t", '<h', 2),
+ 0x06: ("int32_t", '<i', 4),
+ 0x08: ("FP16", '<h', 2),
+ 0x07: ("float", '<f', 4)}
+
+ @staticmethod
+ def get_id_from_cstring(name):
+ """Return variable type id given the C-storage name"""
+ for key in LogTocElement.types.keys():
+ if (LogTocElement.types[key][0] == name):
+ return key
+ raise KeyError("Type [%s] not found in LogTocElement.types!" % name)
+
+ @staticmethod
+ def get_cstring_from_id(ident):
+ """Return the C-storage name given the variable type id"""
+ try:
+ return LogTocElement.types[ident][0]
+ except KeyError:
+ raise KeyError("Type [%d] not found in LogTocElement.types"
+ "!" % ident)
+
+ @staticmethod
+ def get_size_from_id(ident):
+ """Return the size in bytes given the variable type id"""
+ try:
+ return LogTocElement.types[ident][2]
+ except KeyError:
+ raise KeyError("Type [%d] not found in LogTocElement.types"
+ "!" % ident)
+
+ @staticmethod
+ def get_unpack_string_from_id(ident):
+ """Return the Python unpack string given the variable type id"""
+ try:
+ return LogTocElement.types[ident][1]
+ except KeyError:
+ raise KeyError("Type [%d] not found in LogTocElement.types!" % ident)
+
+ def __init__(self, data=None):
+ """TocElement creator. Data is the binary payload of the element."""
+
+ if (data):
+ strs = struct.unpack("s" * len(data[2:]), data[2:])
+ strs = ("{}" * len(strs)).format(*strs).split("\0")
+ self.group = strs[0]
+ self.name = strs[1]
+
+ self.ident = ord(data[0])
+
+ self.ctype = LogTocElement.get_cstring_from_id(ord(data[1]))
+ self.pytype = LogTocElement.get_unpack_string_from_id(ord(data[1]))
+
+ self.access = ord(data[1]) & 0x10
+
+
+class Log():
+ """Create log configuration"""
+
+ # These codes can be decoded using os.stderror, but
+ # some of the text messages will look very strange
+ # in the UI, so they are redefined here
+ _err_codes = {
+ errno.ENOMEM: "No more memory available",
+ errno.ENOEXEC: "Command not found",
+ errno.ENOENT: "No such block id",
+ errno.E2BIG: "Block too large",
+ errno.EEXIST: "Block already exists"
+ }
+
+ def __init__(self, crazyflie=None):
+ self.log_blocks = []
+ # Called with newly created blocks
+ self.block_added_cb = Caller()
+
+ self.cf = crazyflie
+ self.toc = None
+ self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
+
+ self.toc_updated = Caller()
+ self.state = IDLE
+ self.fake_toc_crc = 0xDEADBEEF
+
+ self._refresh_callback = None
+ self._toc_cache = None
+
+ def add_config(self, logconf):
+ """Add a log configuration to the logging framework.
+
+ When doing this the contents of the log configuration will be validated
+ and listeners for new log configurations will be notified. When
+ validating the configuration the variables are checked against the TOC
+ to see that they actually exist. If they don't then the configuration
+ cannot be used. Since a valid TOC is required, a Crazyflie has to be
+ connected when calling this method, otherwise it will fail."""
+
+ if not self.cf.link:
+ logger.error("Cannot add configs without being connected to a "
+ "Crazyflie!")
+ return
+
+ # If the log configuration contains variables that we added without
+ # type (i.e we want the stored as type for fetching as well) then
+ # resolve this now and add them to the block again.
+ for name in logconf.default_fetch_as:
+ var = self.toc.get_element_by_complete_name(name)
+ if not var:
+ logger.warning("%s not in TOC, this block cannot be used!", name)
+ logconf.valid = False
+ raise KeyError("Variable {} not in TOC".format(name))
+ # Now that we know what type this variable has, add it to the log
+ # config again with the correct type
+ logconf.add_variable(name, var.ctype)
+
+ # Now check that all the added variables are in the TOC and that
+ # the total size constraint of a data packet with logging data is
+ # not
+ size = 0
+ for var in logconf.variables:
+ size += LogTocElement.get_size_from_id(var.fetch_as)
+ # Check that we are able to find the variable in the TOC so
+ # we can return error already now and not when the config is sent
+ if var.is_toc_variable():
+ if (self.toc.get_element_by_complete_name(var.name) is None):
+ logger.warning("Log: %s not in TOC, this block cannot be"
+ " used!", var.name)
+ logconf.valid = False
+ raise KeyError("Variable {} not in TOC".format(var.name))
+
+ if (size <= MAX_LOG_DATA_PACKET_SIZE and
+ (logconf.period > 0 and logconf.period < 0xFF)):
+ logconf.valid = True
+ logconf.cf = self.cf
+ self.log_blocks.append(logconf)
+ self.block_added_cb.call(logconf)
+ else:
+ logconf.valid = False
+ raise AttributeError("The log configuration is too large or has an invalid parameter")
+
+ def refresh_toc(self, refresh_done_callback, toc_cache):
+ """Start refreshing the table of loggale variables"""
+
+ self._toc_cache = toc_cache
+ self._refresh_callback = refresh_done_callback
+ self.toc = None
+
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
+ pk.data = (CMD_RESET_LOGGING, )
+ self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING,))
+
+ def _find_block(self, id):
+ for block in self.log_blocks:
+ if block.id == id:
+ return block
+ return None
+
+ def _new_packet_cb(self, packet):
+ """Callback for newly arrived packets with TOC information"""
+ chan = packet.channel
+ cmd = packet.datal[0]
+ payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
+
+ if (chan == CHAN_SETTINGS):
+ id = ord(payload[0])
+ error_status = ord(payload[1])
+ block = self._find_block(id)
+ if (cmd == CMD_CREATE_BLOCK):
+ if (block is not None):
+ if error_status == 0 or error_status == errno.EEXIST:
+ if not block.added:
+ logger.debug("Have successfully added id=%d", id)
+
+ pk = CRTPPacket()
+ pk.set_header(5, CHAN_SETTINGS)
+ pk.data = (CMD_START_LOGGING, id, block.period)
+ self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, id))
+ block.added = True
+ else:
+ msg = self._err_codes[error_status]
+ logger.warning("Error %d when adding id=%d (%s)", error_status, id, msg)
+ block.err_no = error_status
+ block.added_cb.call(False)
+ block.error_cb.call(block, msg)
+
+ else:
+ logger.warning("No LogEntry to assign block to !!!")
+ if (cmd == CMD_START_LOGGING):
+ if (error_status == 0x00):
+ logger.info("Have successfully started logging for id=%d", id)
+ if block:
+ block.started = True
+
+ else:
+ msg = self._err_codes[error_status]
+ logger.warning("Error %d when starting id=%d (%s)", error_status, id, msg)
+ if block:
+ block.err_no = error_status
+ block.started_cb.call(False)
+ # This is a temporary fix, we are adding a new issue
+ # for this. For some reason we get an error back after
+ # the block has been started and added. This will show
+ # an error in the UI, but everything is still working.
+ #block.error_cb.call(block, msg)
+
+ if (cmd == CMD_STOP_LOGGING):
+ if (error_status == 0x00):
+ logger.info("Have successfully stopped logging for id=%d", id)
+ if block:
+ block.started = False
+
+ if (cmd == CMD_DELETE_BLOCK):
+ # Accept deletion of a block that isn't added. This could
+ # happen due to timing (i.e add/start/delete in fast sequence)
+ if error_status == 0x00 or error_status == errno.ENOENT:
+ logger.info("Have successfully deleted id=%d", id)
+ if block:
+ block.started = False
+ block.added = False
+
+ if (cmd == CMD_RESET_LOGGING):
+ # Guard against multiple responses due to re-sending
+ if not self.toc:
+ logger.debug("Logging reset, continue with TOC download")
+ self.log_blocks = []
+
+ self.toc = Toc()
+ toc_fetcher = TocFetcher(self.cf, LogTocElement,
+ CRTPPort.LOGGING,
+ self.toc, self._refresh_callback,
+ self._toc_cache)
+ toc_fetcher.start()
+
+ if (chan == CHAN_LOGDATA):
+ chan = packet.channel
+ id = packet.datal[0]
+ block = self._find_block(id)
+ timestamps = struct.unpack("<BBB", packet.data[1:4])
+ timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
+ logdata = packet.data[4:]
+ if (block is not None):
+ block.unpack_log_data(logdata, timestamp)
+ else:
+ logger.warning("Error no LogEntry to handle id=%d", id)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..40ace216e4a3a559c79ba9354e98c46ae7d1585c
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py
new file mode 100755
index 0000000000000000000000000000000000000000..d2ccacdc3b1eefdf71e0e5d02d3249a587df5c95
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py
@@ -0,0 +1,782 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Enables flash access to the Crazyflie.
+
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Memory', 'MemoryElement']
+
+import struct
+import errno
+from Queue import Queue
+from threading import Lock
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+from cflib.utils.callbacks import Caller
+from binascii import crc32
+import binascii
+
+# Channels used for the logging port
+CHAN_INFO = 0
+CHAN_READ = 1
+CHAN_WRITE = 2
+
+# Commands used when accessing the Settings port
+CMD_INFO_VER = 0
+CMD_INFO_NBR = 1
+CMD_INFO_DETAILS = 2
+
+# The max size of a CRTP packet payload
+MAX_LOG_DATA_PACKET_SIZE = 30
+
+import logging
+logger = logging.getLogger(__name__)
+
+class MemoryElement(object):
+ """A memory """
+
+ TYPE_I2C = 0
+ TYPE_1W = 1
+ TYPE_DRIVER_LED = 0x10
+
+ def __init__(self, id, type, size, mem_handler):
+ """Initialize the element with default values"""
+ self.id = id
+ self.type = type
+ self.size = size
+ self.mem_handler = mem_handler
+
+ @staticmethod
+ def type_to_string(t):
+ """Get string representation of memory type"""
+ if t == MemoryElement.TYPE_I2C:
+ return "I2C"
+ if t == MemoryElement.TYPE_1W:
+ return "1-wire"
+ if t == MemoryElement.TYPE_DRIVER_LED:
+ return "LED driver"
+ return "Unknown"
+
+ def new_data(self, mem, addr, data):
+ logger.info("New data, but not OW mem")
+
+ def __str__(self):
+ """Generate debug string for memory"""
+ return ("Memory: id={}, type={}, size={}".format(
+ self.id, MemoryElement.type_to_string(self.type), self.size))
+
+
+class LED:
+ """Used to set color/intensity of one LED in the LED-ring"""
+ def __init__(self):
+ """Initialize to off"""
+ self.r = 0
+ self.g = 0
+ self.b = 0
+ self.intensity = 100
+
+ def set(self, r, g, b, intensity=None):
+ """Set the R/G/B and optionally intensity in one call"""
+ self.r = r
+ self.g = g
+ self.b = b
+ if intensity:
+ self.intensity = intensity
+
+class LEDDriverMemory(MemoryElement):
+ """Memory interface for using the LED-ring mapped memory for setting RGB
+ values for all the LEDs in the ring"""
+ def __init__(self, id, type, size, mem_handler):
+ """Initialize with 12 LEDs"""
+ super(LEDDriverMemory, self).__init__(id=id, type=type, size=size,
+ mem_handler=mem_handler)
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+
+ self.leds = []
+ for i in range(12):
+ self.leds.append(LED())
+
+ def new_data(self, mem, addr, data):
+ """Callback for when new memory data has been fetched"""
+ if mem.id == self.id:
+ logger.info("Got new data from the LED driver, but we don't care.")
+
+ def write_data(self, write_finished_cb):
+ """Write the saved LED-ring data to the Crazyflie"""
+ self._write_finished_cb = write_finished_cb
+ data = ()
+ for led in self.leds:
+ # In order to fit all the LEDs in one radio packet RGB565 is used
+ # to compress the colors. The calculations below converts 3 bytes
+ # RGB into 2 bytes RGB565. Then shifts the value of each color to
+ # LSB, applies the intensity and shifts them back for correct
+ # alignment on 2 bytes.
+ R5 = (int)((((int(led.r) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * led.intensity/100
+ G6 = (int)((((int(led.g) & 0xFF) * 253 + 505) >> 10) & 0x3F) * led.intensity/100
+ B5 = (int)((((int(led.b) & 0xFF) * 249 + 1014) >> 11) & 0x1F) * led.intensity/100
+ tmp = (R5 << 11) | (G6 << 5) | (B5 << 0)
+ data += (tmp >> 8, tmp & 0xFF)
+ self.mem_handler.write(self, 0x00, data, flush_queue=True)
+
+ def update(self, update_finished_cb):
+ """Request an update of the memory content"""
+ if not self._update_finished_cb:
+ self._update_finished_cb = update_finished_cb
+ self.valid = False
+ logger.info("Updating content of memory {}".format(self.id))
+ # Start reading the header
+ self.mem_handler.read(self, 0, 16)
+
+ def write_done(self, mem, addr):
+ if self._write_finished_cb and mem.id == self.id:
+ logger.info("Write to LED driver done")
+ self._write_finished_cb(self, addr)
+ self._write_finished_cb = None
+
+ def disconnect(self):
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+
+
+class I2CElement(MemoryElement):
+ def __init__(self, id, type, size, mem_handler):
+ super(I2CElement, self).__init__(id=id, type=type, size=size, mem_handler=mem_handler)
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+ self.elements = {}
+ self.valid = False
+
+ def new_data(self, mem, addr, data):
+ """Callback for when new memory data has been fetched"""
+ if mem.id == self.id:
+ if addr == 0:
+ done = False
+ # Check for header
+ if data[0:4] == "0xBC":
+ logger.info("Got new data: {}".format(data))
+ [self.elements["version"],
+ self.elements["radio_channel"],
+ self.elements["radio_speed"],
+ self.elements["pitch_trim"],
+ self.elements["roll_trim"]] = struct.unpack("<BBBff", data[4:15])
+ if self.elements["version"] == 0:
+ done = True
+ elif self.elements["version"] == 1:
+ self.datav0 = data
+ self.mem_handler.read(self, 16, 5)
+
+ if addr == 16:
+ [radio_address_upper,
+ radio_address_lower] = struct.unpack("<BI", self.datav0[15] + data[0:4])
+ self.elements["radio_address"] = int(radio_address_upper) << 32 | radio_address_lower
+
+ logger.info(self.elements)
+ data = self.datav0 + data
+ done = True
+
+ if done:
+ if self._checksum256(data[:len(data)-1]) == ord(data[len(data)-1]):
+ self.valid = True
+ if self._update_finished_cb:
+ self._update_finished_cb(self)
+ self._update_finished_cb = None
+
+ def _checksum256(self, st):
+ return reduce(lambda x, y: x + y, map(ord, st)) % 256
+
+ def write_data(self, write_finished_cb):
+ if self.elements["version"] == 0:
+ data = (0x00, self.elements["radio_channel"], self.elements["radio_speed"],
+ self.elements["pitch_trim"], self.elements["roll_trim"])
+ image = struct.pack("<BBBff", *data)
+ elif self.elements["version"] == 1:
+ data = (0x01, self.elements["radio_channel"], self.elements["radio_speed"],
+ self.elements["pitch_trim"], self.elements["roll_trim"],
+ self.elements["radio_address"] >> 32, self.elements["radio_address"] & 0xFFFFFFFF)
+ image = struct.pack("<BBBffBI", *data)
+ # Adding some magic:
+ image = "0xBC" + image
+ image += struct.pack("B", self._checksum256(image))
+
+ self._write_finished_cb = write_finished_cb
+
+ self.mem_handler.write(self, 0x00, struct.unpack("B"*len(image), image))
+
+ def update(self, update_finished_cb):
+ """Request an update of the memory content"""
+ if not self._update_finished_cb:
+ self._update_finished_cb = update_finished_cb
+ self.valid = False
+ logger.info("Updating content of memory {}".format(self.id))
+ # Start reading the header
+ self.mem_handler.read(self, 0, 16)
+
+ def write_done(self, mem, addr):
+ if self._write_finished_cb and mem.id == self.id:
+ self._write_finished_cb(self, addr)
+ self._write_finished_cb = None
+
+ def disconnect(self):
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+
+
+class OWElement(MemoryElement):
+ """Memory class with extra functionality for 1-wire memories"""
+
+ element_mapping = {
+ 1: "Board name",
+ 2: "Board revision",
+ 3: "Custom"
+ }
+
+ def __init__(self, id, type, size, addr, mem_handler):
+ """Initialize the memory with good defaults"""
+ super(OWElement, self).__init__(id=id, type=type, size=size, mem_handler=mem_handler)
+ self.addr = addr
+
+ self.valid = False
+
+ self.vid = None
+ self.pid = None
+ self.name = None
+ self.pins = None
+ self.elements = {}
+
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+
+ self._rev_element_mapping = {}
+ for key in OWElement.element_mapping.keys():
+ self._rev_element_mapping[OWElement.element_mapping[key]] = key
+
+ def new_data(self, mem, addr, data):
+ """Callback for when new memory data has been fetched"""
+ if mem.id == self.id:
+ if addr == 0:
+ if self._parse_and_check_header(data[0:8]):
+ logger.info("--> HEADER OK")
+ if self._parse_and_check_elements(data[9:11]):
+ self.valid = True
+ self._update_finished_cb(self)
+ self._update_finished_cb = None
+ else:
+ # We need to fetch the elements, find out the length
+ (elem_ver, elem_len) = struct.unpack("BB", data[8:10])
+ self.mem_handler.read(self, 8, elem_len + 3)
+ else:
+ logger.info("--> HEADER NOT OK")
+ # Call the update if the CRC check of the header fails, we're done here
+ if self._update_finished_cb:
+ self._update_finished_cb(self)
+ self._update_finished_cb = None
+ elif addr == 0x08:
+ if self._parse_and_check_elements(data):
+ logger.info("--> ELEMENT OK")
+ self.valid = True
+ else:
+ logger.info("--> ELEMENT NOT OK")
+ if self._update_finished_cb:
+ self._update_finished_cb(self)
+ self._update_finished_cb = None
+
+
+
+ def _parse_and_check_elements(self, data):
+ """Parse and check the CRC and length of the elements part of the memory"""
+ (elem_ver, elem_len, crc) = struct.unpack("<BBB", data[0] + data[1] + data[-1])
+ test_crc = crc32(data[:-1]) & 0x0ff
+ elem_data = data[2:-1]
+ if test_crc == crc:
+ while len(elem_data) > 0:
+ (eid, elen) = struct.unpack("BB", elem_data[:2])
+ self.elements[self.element_mapping[eid]] = elem_data[2:2+elen]
+ elem_data = elem_data[2+elen:]
+ return True
+ return False
+
+
+ def write_done(self, mem, addr):
+ if self._write_finished_cb:
+ self._write_finished_cb(self, addr)
+ self._write_finished_cb = None
+
+ def write_data(self, write_finished_cb):
+ # First generate the header part
+ header_data = struct.pack("<BIBB", 0xEB, self.pins, self.vid, self.pid)
+ header_crc = crc32(header_data) & 0x0ff
+ header_data += struct.pack("B", header_crc)
+
+ # Now generate the elements part
+ elem = ""
+ logger.info(self.elements.keys())
+ for element in reversed(self.elements.keys()):
+ elem_string = self.elements[element]
+ #logger.info(">>>> {}".format(elem_string))
+ key_encoding = self._rev_element_mapping[element]
+ elem += struct.pack("BB", key_encoding, len(elem_string))
+ elem += elem_string
+
+ elem_data = struct.pack("BB", 0x00, len(elem))
+ elem_data += elem
+ elem_crc = crc32(elem_data) & 0x0ff
+ elem_data += struct.pack("B", elem_crc)
+
+ data = header_data + elem_data
+
+ # Write data
+ p = ""
+ for s in data:
+ p += "0x{:02X} ".format(ord(s))
+ logger.info(p)
+
+ self.mem_handler.write(self, 0x00, struct.unpack("B"*len(data), data))
+
+ self._write_finished_cb = write_finished_cb
+
+ def update(self, update_finished_cb):
+ """Request an update of the memory content"""
+ if not self._update_finished_cb:
+ self._update_finished_cb = update_finished_cb
+ self.valid = False
+ logger.info("Updating content of memory {}".format(self.id))
+ # Start reading the header
+ self.mem_handler.read(self, 0, 11)
+ #else:
+ # logger.warning("Already in progress of updating memory {}".format(self.id))
+
+ def _parse_and_check_header(self, data):
+ """Parse and check the CRC of the header part of the memory"""
+ #logger.info("Should parse header: {}".format(data))
+ (start, self.pins, self.vid, self.pid, crc) = struct.unpack("<BIBBB", data)
+ test_crc = crc32(data[:-1]) & 0x0ff
+ if start == 0xEB and crc == test_crc:
+ return True
+ return False
+
+ def __str__(self):
+ """Generate debug string for memory"""
+ return ("OW {} ({:02X}:{:02X}): {}".format(
+ self.addr, self.vid, self.pid, self.elements))
+
+ def disconnect(self):
+ self._update_finished_cb = None
+ self._write_finished_cb = None
+
+
+class _ReadRequest:
+ """Class used to handle memory reads that will split up the read in multiple packets in necessary"""
+ MAX_DATA_LENGTH = 20
+
+ def __init__(self, mem, addr, length, cf):
+ """Initialize the object with good defaults"""
+ self.mem = mem
+ self.addr = addr
+ self._bytes_left = length
+ self.data = ""
+ self.cf = cf
+
+ self._current_addr = addr
+
+ def start(self):
+ """Start the fetching of the data"""
+ self._request_new_chunk()
+
+ def resend(self):
+ logger.info("Sending write again...")
+ self._request_new_chunk()
+
+ def _request_new_chunk(self):
+ """Called to request a new chunk of data to be read from the Crazyflie"""
+ # Figure out the length of the next request
+ new_len = self._bytes_left
+ if new_len > _ReadRequest.MAX_DATA_LENGTH:
+ new_len = _ReadRequest.MAX_DATA_LENGTH
+
+ logger.info("Requesting new chunk of {}bytes at 0x{:X}".format(new_len, self._current_addr))
+
+ # Request the data for the next address
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.MEM, CHAN_READ)
+ pk.data = struct.pack("<BIB", self.mem.id, self._current_addr, new_len)
+ reply = struct.unpack("<BBBBB", pk.data[:-1])
+ self.cf.send_packet(pk, expected_reply=reply, timeout=1)
+
+ def add_data(self, addr, data):
+ """Callback when data is received from the Crazyflie"""
+ data_len = len(data)
+ if not addr == self._current_addr:
+ logger.warning("Address did not match when adding data to read request!")
+ return
+
+ # Add the data and calculate the next address to fetch
+ self.data += data
+ self._bytes_left -= data_len
+ self._current_addr += data_len
+
+ if self._bytes_left > 0:
+ self._request_new_chunk()
+ return False
+ else:
+ return True
+
+class _WriteRequest:
+ """Class used to handle memory reads that will split up the read in multiple packets in necessary"""
+ MAX_DATA_LENGTH = 25
+
+ def __init__(self, mem, addr, data, cf):
+ """Initialize the object with good defaults"""
+ self.mem = mem
+ self.addr = addr
+ self._bytes_left = len(data)
+ self._data = data
+ self.data = ""
+ self.cf = cf
+
+ self._current_addr = addr
+
+ self._sent_packet = None
+ self._sent_reply = None
+
+ self._addr_add = 0
+
+ def start(self):
+ """Start the fetching of the data"""
+ self._write_new_chunk()
+
+ def resend(self):
+ logger.info("Sending write again...")
+ self.cf.send_packet(self._sent_packet, expected_reply=self._sent_reply, timeout=1)
+
+ def _write_new_chunk(self):
+ """Called to request a new chunk of data to be read from the Crazyflie"""
+ # Figure out the length of the next request
+ new_len = len(self._data)
+ if new_len > _WriteRequest.MAX_DATA_LENGTH:
+ new_len = _WriteRequest.MAX_DATA_LENGTH
+
+ logger.info("Writing new chunk of {}bytes at 0x{:X}".format(new_len, self._current_addr))
+
+ data = self._data[:new_len]
+ self._data = self._data[new_len:]
+
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.MEM, CHAN_WRITE)
+ pk.data = struct.pack("<BI", self.mem.id, self._current_addr)
+ # Create a tuple used for matching the reply using id and address
+ reply = struct.unpack("<BBBBB", pk.data)
+ self._sent_reply = reply
+ # Add the data
+ pk.data += struct.pack("B"*len(data), *data)
+ self._sent_packet = pk
+ self.cf.send_packet(pk, expected_reply=reply, timeout=1)
+
+ self._addr_add = len(data)
+
+ def write_done(self, addr):
+ """Callback when data is received from the Crazyflie"""
+ if not addr == self._current_addr:
+ logger.warning("Address did not match when adding data to read request!")
+ return
+
+ if len(self._data) > 0:
+ self._current_addr += self._addr_add
+ self._write_new_chunk()
+ return False
+ else:
+ logger.info("This write request is done")
+ return True
+
+class Memory():
+ """Access memories on the Crazyflie"""
+
+ # These codes can be decoded using os.stderror, but
+ # some of the text messages will look very strange
+ # in the UI, so they are redefined here
+ _err_codes = {
+ errno.ENOMEM: "No more memory available",
+ errno.ENOEXEC: "Command not found",
+ errno.ENOENT: "No such block id",
+ errno.E2BIG: "Block too large",
+ errno.EEXIST: "Block already exists"
+ }
+
+ def __init__(self, crazyflie=None):
+ """Instantiate class and connect callbacks"""
+ self.mems = []
+ # Called when new memories have been added
+ self.mem_added_cb = Caller()
+ # Called when new data has been read
+ self.mem_read_cb = Caller()
+
+ self.mem_write_cb = Caller()
+
+ self.cf = crazyflie
+ self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
+
+ self._refresh_callback = None
+ self._fetch_id = 0
+ self.nbr_of_mems = 0
+ self._ow_mem_fetch_index = 0
+ self._elem_data = ()
+ self._read_requests = {}
+ self._read_requests_lock = Lock()
+ self._write_requests = {}
+ self._write_requests_lock = Lock()
+ self._ow_mems_left_to_update = []
+
+ self._getting_count = False
+
+ def _mem_update_done(self, mem):
+ """Callback from each individual memory (only 1-wire) when reading of header/elements are done"""
+ if mem.id in self._ow_mems_left_to_update:
+ self._ow_mems_left_to_update.remove(mem.id)
+
+ logger.info(mem)
+
+ if len(self._ow_mems_left_to_update) == 0:
+ if self._refresh_callback:
+ self._refresh_callback()
+ self._refresh_callback = None
+
+ def get_mem(self, id):
+ """Fetch the memory with the supplied id"""
+ for m in self.mems:
+ if m.id == id:
+ return m
+
+ return None
+
+ def get_mems(self, type):
+ """Fetch all the memories of the supplied type"""
+ ret = ()
+ for m in self.mems:
+ if m.type == type:
+ ret += (m, )
+
+ return ret
+
+ def ow_search(self, vid=0xBC, pid=None, name=None):
+ """Search for specific memory id/name and return it"""
+ for m in self.get_mems(MemoryElement.TYPE_1W):
+ if pid and m.pid == pid or name and m.name == name:
+ return m
+
+ return None
+
+ def write(self, memory, addr, data, flush_queue=False):
+ """Write the specified data to the given memory at the given address"""
+ wreq = _WriteRequest(memory, addr, data, self.cf)
+ if not memory.id in self._write_requests:
+ self._write_requests[memory.id] = []
+
+ # Workaround until we secure the uplink and change messages for
+ # mems to non-blocking
+ self._write_requests_lock.acquire()
+ if flush_queue:
+ self._write_requests[memory.id] = self._write_requests[memory.id][:1]
+ self._write_requests[memory.id].insert(len(self._write_requests), wreq)
+ if len(self._write_requests[memory.id]) == 1:
+ wreq.start()
+ self._write_requests_lock.release()
+
+ return True
+
+
+ def read(self, memory, addr, length):
+ """Read the specified amount of bytes from the given memory at the given address"""
+ if memory.id in self._read_requests:
+ logger.warning("There is already a read operation ongoing for memory id {}".format(memory.id))
+ return False
+
+ rreq = _ReadRequest(memory, addr, length, self.cf)
+ self._read_requests[memory.id] = rreq
+
+ rreq.start()
+
+ return True
+
+ def refresh(self, refresh_done_callback):
+ """Start fetching all the detected memories"""
+ self._refresh_callback = refresh_done_callback
+ self._fetch_id = 0
+ for m in self.mems:
+ try:
+ self.mem_read_cb.remove_callback(m.new_data)
+ m.disconnect()
+ except Exception as e:
+ logger.info("Error when removing memory after update: {}".format(e))
+ self.mems = []
+
+ self.nbr_of_mems = 0
+ self._getting_count = False
+
+ logger.info("Requesting number of memories")
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.MEM, CHAN_INFO)
+ pk.data = (CMD_INFO_NBR, )
+ self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,))
+
+ def _new_packet_cb(self, packet):
+ """Callback for newly arrived packets for the memory port"""
+ chan = packet.channel
+ cmd = packet.datal[0]
+ payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
+ #logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload)))
+
+ if chan == CHAN_INFO:
+ if cmd == CMD_INFO_NBR:
+ self.nbr_of_mems = ord(payload[0])
+ logger.info("{} memories found".format(self.nbr_of_mems))
+
+ # Start requesting information about the memories, if there are any...
+ if self.nbr_of_mems > 0:
+ if not self._getting_count:
+ self._getting_count = True
+ logger.info("Requesting first id")
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.MEM, CHAN_INFO)
+ pk.data = (CMD_INFO_DETAILS, 0)
+ self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0))
+ else:
+ self._refresh_callback()
+
+ if cmd == CMD_INFO_DETAILS:
+
+ # Did we get a good reply, otherwise try again:
+ if len(payload) < 5:
+ # Workaround for 1-wire bug when memory is detected
+ # but updating the info crashes the communication with
+ # the 1-wire. Fail by saying we only found 1 memory (the I2C).
+ logger.error("-------->Got good count, but no info on mem!")
+ self.nbr_of_mems = 1
+ if self._refresh_callback:
+ self._refresh_callback()
+ self._refresh_callback = None
+ return
+
+ # Create information about a new memory
+ # Id - 1 byte
+ mem_id = ord(payload[0])
+ # Type - 1 byte
+ mem_type = ord(payload[1])
+ # Size 4 bytes (as addr)
+ mem_size = struct.unpack("I", payload[2:6])[0]
+ # Addr (only valid for 1-wire?)
+ mem_addr_raw = struct.unpack("B"*8, payload[6:14])
+ mem_addr = ""
+ for m in mem_addr_raw:
+ mem_addr += "{:02X}".format(m)
+
+ if (not self.get_mem(mem_id)):
+ if mem_type == MemoryElement.TYPE_1W:
+ mem = OWElement(id=mem_id, type=mem_type, size=mem_size,
+ addr=mem_addr, mem_handler=self)
+ self.mem_read_cb.add_callback(mem.new_data)
+ self.mem_write_cb.add_callback(mem.write_done)
+ self._ow_mems_left_to_update.append(mem.id)
+ elif mem_type == MemoryElement.TYPE_I2C:
+ mem = I2CElement(id=mem_id, type=mem_type, size=mem_size,
+ mem_handler=self)
+ self.mem_read_cb.add_callback(mem.new_data)
+ self.mem_write_cb.add_callback(mem.write_done)
+ elif mem_type == MemoryElement.TYPE_DRIVER_LED:
+ mem = LEDDriverMemory(id=mem_id, type=mem_type,
+ size=mem_size, mem_handler=self)
+ logger.info(mem)
+ self.mem_read_cb.add_callback(mem.new_data)
+ self.mem_write_cb.add_callback(mem.write_done)
+ else:
+ mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self)
+ logger.info(mem)
+ self.mems.append(mem)
+ self.mem_added_cb.call(mem)
+ #logger.info(mem)
+
+ self._fetch_id = mem_id + 1
+
+ if self.nbr_of_mems - 1 >= self._fetch_id:
+ logger.info("Requesting information about memory {}".format(self._fetch_id))
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.MEM, CHAN_INFO)
+ pk.data = (CMD_INFO_DETAILS, self._fetch_id)
+ self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id))
+ else:
+ logger.info("Done getting all the memories, start reading the OWs")
+ ows = self.get_mems(MemoryElement.TYPE_1W)
+ # If there are any OW mems start reading them, otherwise we are done
+ for ow_mem in self.get_mems(MemoryElement.TYPE_1W):
+ ow_mem.update(self._mem_update_done)
+ if len (self.get_mems(MemoryElement.TYPE_1W)) == 0:
+ if self._refresh_callback:
+ self._refresh_callback()
+ self._refresh_callback = None
+
+ if chan == CHAN_WRITE:
+ id = cmd
+ (addr, status) = struct.unpack("<IB", payload[0:5])
+ logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status))
+ # Find the read request
+ if id in self._write_requests:
+ self._write_requests_lock.acquire()
+ wreq = self._write_requests[id][0]
+ if status == 0:
+ if wreq.write_done(addr):
+ #self._write_requests.pop(id, None)
+ # Remove the first item
+ self._write_requests[id].pop(0)
+ self.mem_write_cb.call(wreq.mem, wreq.addr)
+
+ # Get a new one to start (if there are any)
+ if len(self._write_requests[id]) > 0:
+ self._write_requests[id][0].start()
+
+ else:
+ logger.info("Status {}: write resending...".format(status))
+ wreq.resend()
+ self._write_requests_lock.release()
+
+ if chan == CHAN_READ:
+ id = cmd
+ (addr, status) = struct.unpack("<IB", payload[0:5])
+ data = struct.unpack("B"*len(payload[5:]), payload[5:])
+ logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data))
+ # Find the read request
+ if id in self._read_requests:
+ logger.info("READING: We are still interested in request for mem {}".format(id))
+ rreq = self._read_requests[id]
+ if status == 0:
+ if rreq.add_data(addr, payload[5:]):
+ self._read_requests.pop(id, None)
+ self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
+ else:
+ logger.info("Status {}: resending...".format(status))
+ rreq.resend()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d61009696ecb5f8d5e8365430732bb8265f8f88e
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py
new file mode 100755
index 0000000000000000000000000000000000000000..8ce57240e4f50a874cbada71becc33eb6a35ceae
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py
@@ -0,0 +1,315 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Enables reading/writing of parameter values to/from the Crazyflie.
+
+When a Crazyflie is connected it's possible to download a TableOfContent of all
+the parameters that can be written/read.
+
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Param', 'ParamTocElement']
+
+from cflib.utils.callbacks import Caller
+import struct
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+from .toc import Toc, TocFetcher
+from threading import Thread, Lock
+
+from Queue import Queue
+
+import logging
+logger = logging.getLogger(__name__)
+
+#Possible states
+IDLE = 0
+WAIT_TOC = 1
+WAIT_READ = 2
+WAIT_WRITE = 3
+
+TOC_CHANNEL = 0
+READ_CHANNEL = 1
+WRITE_CHANNEL = 2
+
+# TOC access command
+TOC_RESET = 0
+TOC_GETNEXT = 1
+TOC_GETCRC32 = 2
+
+
+# One element entry in the TOC
+class ParamTocElement:
+ """An element in the Log TOC."""
+
+ RW_ACCESS = 0
+ RO_ACCESS = 1
+
+ types = {0x08: ("uint8_t", '<B'),
+ 0x09: ("uint16_t", '<H'),
+ 0x0A: ("uint32_t", '<L'),
+ 0x0B: ("uint64_t", '<Q'),
+ 0x00: ("int8_t", '<b'),
+ 0x01: ("int16_t", '<h'),
+ 0x02: ("int32_t", '<i'),
+ 0x03: ("int64_t", '<q'),
+ 0x05: ("FP16", ''),
+ 0x06: ("float", '<f'),
+ 0x07: ("double", '<d')}
+
+ def __init__(self, data=None):
+ """TocElement creator. Data is the binary payload of the element."""
+ if (data):
+ strs = struct.unpack("s" * len(data[2:]), data[2:])
+ strs = ("{}" * len(strs)).format(*strs).split("\0")
+ self.group = strs[0]
+ self.name = strs[1]
+
+ self.ident = ord(data[0])
+
+ self.ctype = self.types[ord(data[1]) & 0x0F][0]
+ self.pytype = self.types[ord(data[1]) & 0x0F][1]
+
+ if ((ord(data[1]) & 0x40) != 0):
+ self.access = ParamTocElement.RO_ACCESS
+ else:
+ self.access = ParamTocElement.RW_ACCESS
+
+ def get_readable_access(self):
+ if (self.access == ParamTocElement.RO_ACCESS):
+ return "RO"
+ return "RW"
+
+
+class Param():
+ """
+ Used to read and write parameter values in the Crazyflie.
+ """
+
+ toc = Toc()
+
+ def __init__(self, crazyflie):
+ self.cf = crazyflie
+ self.param_update_callbacks = {}
+ self.group_update_callbacks = {}
+ self.all_update_callback = Caller()
+ self.param_updater = None
+
+ self.param_updater = _ParamUpdater(self.cf, self._param_updated)
+ self.param_updater.start()
+
+ self.cf.disconnected.add_callback(self.param_updater.close)
+
+ self.all_updated = Caller()
+ self._have_updated = False
+
+ self.values = {}
+
+ def request_update_of_all_params(self):
+ """Request an update of all the parameters in the TOC"""
+ for group in self.toc.toc:
+ for name in self.toc.toc[group]:
+ complete_name = "%s.%s" % (group, name)
+ self.request_param_update(complete_name)
+
+ def _check_if_all_updated(self):
+ """Check if all parameters from the TOC has at least been fetched
+ once"""
+ for g in self.toc.toc:
+ if not g in self.values:
+ return False
+ for n in self.toc.toc[g]:
+ if not n in self.values[g]:
+ return False
+
+ return True
+
+ def _param_updated(self, pk):
+ """Callback with data for an updated parameter"""
+ var_id = pk.datal[0]
+ element = self.toc.get_element_by_id(var_id)
+ if element:
+ s = struct.unpack(element.pytype, pk.data[1:])[0]
+ s = s.__str__()
+ complete_name = "%s.%s" % (element.group, element.name)
+
+ # Save the value for synchronous access
+ if not element.group in self.values:
+ self.values[element.group] = {}
+ self.values[element.group][element.name] = s
+
+ # This will only be called once
+ if self._check_if_all_updated() and not self._have_updated:
+ self._have_updated = True
+ self.all_updated.call()
+
+ logger.debug("Updated parameter [%s]" % complete_name)
+ if complete_name in self.param_update_callbacks:
+ self.param_update_callbacks[complete_name].call(complete_name, s)
+ if element.group in self.group_update_callbacks:
+ self.group_update_callbacks[element.group].call(complete_name, s)
+ self.all_update_callback.call(complete_name, s)
+ else:
+ logger.debug("Variable id [%d] not found in TOC", var_id)
+
+ def remove_update_callback(self, group, name=None, cb=None):
+ """Remove the supplied callback for a group or a group.name"""
+ if not cb:
+ return
+
+ if not name:
+ if group in self.group_update_callbacks:
+ self.group_update_callbacks[group].remove_callback(cb)
+ else:
+ paramname = "{}.{}".format(group, name)
+ if paramname in self.param_update_callbacks:
+ self.param_update_callbacks[paramname].remove_callback(cb)
+
+ def add_update_callback(self, group=None, name=None, cb=None):
+ """
+ Add a callback for a specific parameter name. This callback will be
+ executed when a new value is read from the Crazyflie.
+ """
+ if not group and not name:
+ self.all_update_callback.add_callback(cb)
+ elif not name:
+ if not group in self.group_update_callbacks:
+ self.group_update_callbacks[group] = Caller()
+ self.group_update_callbacks[group].add_callback(cb)
+ else:
+ paramname = "{}.{}".format(group, name)
+ if not paramname in self.param_update_callbacks:
+ self.param_update_callbacks[paramname] = Caller()
+ self.param_update_callbacks[paramname].add_callback(cb)
+
+ def refresh_toc(self, refresh_done_callback, toc_cache):
+ """
+ Initiate a refresh of the parameter TOC.
+ """
+ self.toc = Toc()
+ toc_fetcher = TocFetcher(self.cf, ParamTocElement,
+ CRTPPort.PARAM, self.toc,
+ refresh_done_callback, toc_cache)
+ toc_fetcher.start()
+
+ def disconnected(self, uri):
+ """Disconnected callback from Crazyflie API"""
+ self.param_updater.close()
+ self._have_updated = False
+
+ def request_param_update(self, complete_name):
+ """
+ Request an update of the value for the supplied parameter.
+ """
+ self.param_updater.request_param_update(
+ self.toc.get_element_id(complete_name))
+
+ def set_value(self, complete_name, value):
+ """
+ Set the value for the supplied parameter.
+ """
+ element = self.toc.get_element_by_complete_name(complete_name)
+
+ if not element:
+ logger.warning("Cannot set value for [%s], it's not in the TOC!",
+ complete_name)
+ raise KeyError("{} not in param TOC".format(complete_name))
+ elif element.access == ParamTocElement.RO_ACCESS:
+ logger.debug("[%s] is read only, no trying to set value", complete_name)
+ raise AttributeError("{} is read-only!".format(complete_name))
+ else:
+ varid = element.ident
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
+ pk.data = struct.pack('<B', varid)
+ pk.data += struct.pack(element.pytype, eval(value))
+ self.param_updater.request_param_setvalue(pk)
+
+
+class _ParamUpdater(Thread):
+ """This thread will update params through a queue to make sure that we
+ get back values"""
+ def __init__(self, cf, updated_callback):
+ """Initialize the thread"""
+ Thread.__init__(self)
+ self.setDaemon(True)
+ self.wait_lock = Lock()
+ self.cf = cf
+ self.updated_callback = updated_callback
+ self.request_queue = Queue()
+ self.cf.add_port_callback(CRTPPort.PARAM, self._new_packet_cb)
+ self._should_close = False
+ self._req_param = -1
+
+ def close(self, uri):
+ # First empty the queue from all packets
+ while not self.request_queue.empty():
+ self.request_queue.get()
+ # Then force an unlock of the mutex if we are waiting for a packet
+ # we didn't get back due to a disconnect for example.
+ try:
+ self.wait_lock.release()
+ except:
+ pass
+
+ def request_param_setvalue(self, pk):
+ """Place a param set value request on the queue. When this is sent to
+ the Crazyflie it will answer with the update param value. """
+ self.request_queue.put(pk)
+
+ def _new_packet_cb(self, pk):
+ """Callback for newly arrived packets"""
+ if pk.channel == READ_CHANNEL or pk.channel == WRITE_CHANNEL:
+ var_id = pk.datal[0]
+ if (pk.channel != TOC_CHANNEL and self._req_param == var_id
+ and pk is not None):
+ self.updated_callback(pk)
+ self._req_param = -1
+ try:
+ self.wait_lock.release()
+ except:
+ pass
+
+ def request_param_update(self, var_id):
+ """Place a param update request on the queue"""
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
+ pk.data = struct.pack('<B', var_id)
+ logger.debug("Requesting request to update param [%d]", var_id)
+ self.request_queue.put(pk)
+
+ def run(self):
+ while not self._should_close:
+ pk = self.request_queue.get() # Wait for request update
+ self.wait_lock.acquire()
+ if self.cf.link:
+ self._req_param = pk.datal[0]
+ self.cf.send_packet(pk, expected_reply=(pk.datat[0:2]))
+ else:
+ self.wait_lock.release()
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..2f3cf00bc16fad78ecb3bbf4bb7cdd384f04d9ac
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.py
new file mode 100755
index 0000000000000000000000000000000000000000..cc6d06ccd2a479fc4e686b140dde58d41ddd84d8
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Used for sending control setpoints to the Crazyflie
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PlatformService']
+
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
+import struct
+
+
+class PlatformService():
+ """
+ Used for sending control setpoints to the Crazyflie
+ """
+
+ def __init__(self, crazyflie=None):
+ """
+ Initialize the platform object.
+ """
+ self._cf = crazyflie
+
+ def set_continous_wave(self, enabled):
+ """
+ Enable/disable the client side X-mode. When enabled this recalculates
+ the setpoints before sending them to the Crazyflie.
+ """
+ pk = CRTPPacket()
+ pk.set_header(CRTPPort.PLATFORM, 0)
+ pk.data = (0, enabled)
+ self._cf.send_packet(pk)
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e0f185d94aab0d8b11573478ed5d61d8a63cfdf4
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.py
new file mode 100755
index 0000000000000000000000000000000000000000..763b3aa270a469e75c285a98dd9f23941dbb4345
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.py
@@ -0,0 +1,203 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+A generic TableOfContents module that is used to fetch, store and minipulate
+a TOC for logging or parameters.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['TocElement', 'Toc', 'TocFetcher']
+
+from cflib.crtp.crtpstack import CRTPPacket
+import struct
+
+import logging
+logger = logging.getLogger(__name__)
+
+TOC_CHANNEL = 0
+
+# Commands used when accessing the Table of Contents
+CMD_TOC_ELEMENT = 0
+CMD_TOC_INFO = 1
+
+# Possible states when receiving TOC
+IDLE = "IDLE"
+GET_TOC_INFO = "GET_TOC_INFO"
+GET_TOC_ELEMENT = "GET_TOC_ELEMENT"
+
+
+class TocElement:
+ """An element in the TOC."""
+ RW_ACCESS = 0
+ RO_ACCESS = 1
+
+ ident = 0
+ group = ""
+ name = ""
+ ctype = ""
+ pytype = ""
+ access = RO_ACCESS
+
+
+class Toc:
+ """Container for TocElements."""
+
+ def __init__(self):
+ self.toc = {}
+
+ def clear(self):
+ """Clear the TOC"""
+ self.toc = {}
+
+ def add_element(self, element):
+ """Add a new TocElement to the TOC container."""
+ try:
+ self.toc[element.group][element.name] = element
+ except KeyError:
+ self.toc[element.group] = {}
+ self.toc[element.group][element.name] = element
+
+ def get_element_by_complete_name(self, complete_name):
+ """Get a TocElement element identified by complete name from the
+ container."""
+ try:
+ return self.get_element_by_id(self.get_element_id(complete_name))
+ except ValueError:
+ # Item not found
+ return None
+
+ def get_element_id(self, complete_name):
+ """Get the TocElement element id-number of the element with the
+ supplied name."""
+ [group, name] = complete_name.split(".")
+ element = self.get_element(group, name)
+ if element:
+ return element.ident
+ else:
+ logger.warning("Unable to find variable [%s]", complete_name)
+ return None
+
+ def get_element(self, group, name):
+ """Get a TocElement element identified by name and group from the
+ container."""
+ try:
+ return self.toc[group][name]
+ except KeyError:
+ return None
+
+ def get_element_by_id(self, ident):
+ """Get a TocElement element identified by index number from the
+ container."""
+ for group in self.toc.keys():
+ for name in self.toc[group].keys():
+ if self.toc[group][name].ident == ident:
+ return self.toc[group][name]
+ return None
+
+
+class TocFetcher:
+ """Fetches TOC entries from the Crazyflie"""
+ def __init__(self, crazyflie, element_class, port, toc_holder,
+ finished_callback, toc_cache):
+ self.cf = crazyflie
+ self.port = port
+ self._crc = 0
+ self.requested_index = None
+ self.nbr_of_items = None
+ self.state = None
+ self.toc = toc_holder
+ self._toc_cache = toc_cache
+ self.finished_callback = finished_callback
+ self.element_class = element_class
+
+ def start(self):
+ """Initiate fetching of the TOC."""
+ logger.debug("[%d]: Start fetching...", self.port)
+ # Register callback in this class for the port
+ self.cf.add_port_callback(self.port, self._new_packet_cb)
+
+ # Request the TOC CRC
+ self.state = GET_TOC_INFO
+ pk = CRTPPacket()
+ pk.set_header(self.port, TOC_CHANNEL)
+ pk.data = (CMD_TOC_INFO, )
+ self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
+
+ def _toc_fetch_finished(self):
+ """Callback for when the TOC fetching is finished"""
+ self.cf.remove_port_callback(self.port, self._new_packet_cb)
+ logger.debug("[%d]: Done!", self.port)
+ self.finished_callback()
+
+ def _new_packet_cb(self, packet):
+ """Handle a newly arrived packet"""
+ chan = packet.channel
+ if (chan != 0):
+ return
+ payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
+
+ if (self.state == GET_TOC_INFO):
+ [self.nbr_of_items, self._crc] = struct.unpack("<BI", payload[:5])
+ logger.debug("[%d]: Got TOC CRC, %d items and crc=0x%08X",
+ self.port, self.nbr_of_items, self._crc)
+
+ cache_data = self._toc_cache.fetch(self._crc)
+ if (cache_data):
+ self.toc.toc = cache_data
+ logger.info("TOC for port [%s] found in cache" % self.port)
+ self._toc_fetch_finished()
+ else:
+ self.state = GET_TOC_ELEMENT
+ self.requested_index = 0
+ self._request_toc_element(self.requested_index)
+
+ elif (self.state == GET_TOC_ELEMENT):
+ # Always add new element, but only request new if it's not the
+ # last one.
+ if self.requested_index != ord(payload[0]):
+ return
+ self.toc.add_element(self.element_class(payload))
+ logger.debug("Added element [%s]",
+ self.element_class(payload).ident)
+ if (self.requested_index < (self.nbr_of_items - 1)):
+ logger.debug("[%d]: More variables, requesting index %d",
+ self.port, self.requested_index + 1)
+ self.requested_index = self.requested_index + 1
+ self._request_toc_element(self.requested_index)
+ else: # No more variables in TOC
+ self._toc_cache.insert(self._crc, self.toc.toc)
+ self._toc_fetch_finished()
+
+ def _request_toc_element(self, index):
+ """Request information about a specific item in the TOC"""
+ logger.debug("Requesting index %d on port %d", index, self.port)
+ pk = CRTPPacket()
+ pk.set_header(self.port, TOC_CHANNEL)
+ pk.data = (CMD_TOC_ELEMENT, index)
+ self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..aa0e28b806abec352c54ff269edcf545f3adc1a8
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.py
new file mode 100755
index 0000000000000000000000000000000000000000..855cfc62a72c7ca927453f0823e2af384a62b444
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Access the TOC cache for reading/writing. It supports both user
+cache and dist cache.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['TocCache']
+
+import os
+import json
+from glob import glob
+
+import logging
+logger = logging.getLogger(__name__)
+
+from .log import LogTocElement # pylint: disable=W0611
+from .param import ParamTocElement # pylint: disable=W0611
+
+
+
+class TocCache():
+ """
+ Access to TOC cache. To turn of the cache functionality
+ don't supply any directories.
+ """
+ def __init__(self, ro_cache=None, rw_cache=None):
+ self._cache_files = []
+ if (ro_cache):
+ self._cache_files += glob(ro_cache + "/*.json")
+ if (rw_cache):
+ self._cache_files += glob(rw_cache + "/*.json")
+ if not os.path.exists(rw_cache):
+ os.makedirs(rw_cache)
+
+ self._rw_cache = rw_cache
+
+ def fetch(self, crc):
+ """ Try to get a hit in the cache, return None otherwise """
+ cache_data = None
+ pattern = "%08X.json" % crc
+ hit = None
+
+ for name in self._cache_files:
+ if (name.endswith(pattern)):
+ hit = name
+
+ if (hit):
+ try:
+ cache = open(hit)
+ cache_data = json.load(cache,
+ object_hook=self._decoder)
+ cache.close()
+ except Exception as exp:
+ logger.warning("Error while parsing cache file [%s]:%s",
+ hit, str(exp))
+
+ return cache_data
+
+ def insert(self, crc, toc):
+ """ Save a new cache to file """
+ if self._rw_cache:
+ try:
+ filename = "%s/%08X.json" % (self._rw_cache, crc)
+ cache = open(filename, 'w')
+ cache.write(json.dumps(toc, indent=2,
+ default=self._encoder))
+ cache.close()
+ logger.info("Saved cache to [%s]", filename)
+ self._cache_files += [filename]
+ except Exception as exp:
+ logger.warning("Could not save cache to file [%s]: %s",
+ filename, str(exp))
+ else:
+ logger.warning("Could not save cache, no writable directory")
+
+ def _encoder(self, obj):
+ """ Encode a toc element leaf-node """
+ return {'__class__': obj.__class__.__name__,
+ 'ident': obj.ident,
+ 'group': obj.group,
+ 'name': obj.name,
+ 'ctype': obj.ctype,
+ 'pytype': obj.pytype,
+ 'access': obj.access}
+ raise TypeError(repr(obj) + ' is not JSON serializable')
+
+ def _decoder(self, obj):
+ """ Decode a toc element leaf-node """
+ if '__class__' in obj:
+ elem = eval(obj['__class__'])()
+ elem.ident = obj['ident']
+ elem.group = str(obj['group'])
+ elem.name = str(obj['name'])
+ elem.ctype = str(obj['ctype'])
+ elem.pytype = str(obj['pytype'])
+ elem.access = obj['access']
+ return elem
+ return obj
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f7ec39db18ee58402fe076ba5710ded82671d339
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..f46e63cd1a9fd85bf5c148edc4afe7058c1ab076
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.py
@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""Scans and creates communication interfaces."""
+
+__author__ = 'Bitcraze AB'
+__all__ = []
+
+
+import logging
+logger = logging.getLogger(__name__)
+
+from .radiodriver import RadioDriver
+from .udpdriver import UdpDriver
+from .serialdriver import SerialDriver
+from .debugdriver import DebugDriver
+from .usbdriver import UsbDriver
+from .exceptions import WrongUriType
+
+DRIVERS = [RadioDriver, SerialDriver, UdpDriver, DebugDriver, UsbDriver]
+INSTANCES = []
+
+
+def init_drivers(enable_debug_driver=False):
+ """Initialize all the drivers."""
+ for driver in DRIVERS:
+ try:
+ if driver != DebugDriver or enable_debug_driver:
+ INSTANCES.append(driver())
+ except Exception: # pylint: disable=W0703
+ continue
+
+
+def scan_interfaces(address = None):
+ """ Scan all the interfaces for available Crazyflies """
+ available = []
+ found = []
+ for instance in INSTANCES:
+ logger.debug("Scanning: %s", instance)
+ try:
+ found = instance.scan_interface(address)
+ available += found
+ except Exception:
+ raise
+ return available
+
+
+def get_interfaces_status():
+ """Get the status of all the interfaces"""
+ status = {}
+ for instance in INSTANCES:
+ try:
+ status[instance.get_name()] = instance.get_status()
+ except Exception:
+ raise
+ return status
+
+
+def get_link_driver(uri, link_quality_callback=None, link_error_callback=None):
+ """Return the link driver for the given URI. Returns None if no driver
+ was found for the URI or the URI was not well formatted for the matching
+ driver."""
+ for instance in INSTANCES:
+ try:
+ instance.connect(uri, link_quality_callback, link_error_callback)
+ return instance
+ except WrongUriType:
+ continue
+
+ return None
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..69bc9be5a104d24a54da83c47d8b7f4d927a029b
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..b5364f3b7c33df3541279253b0ff507fa6abdd45
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.py
@@ -0,0 +1,96 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+CRTP Driver main class.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['CRTPDriver']
+
+
+class CRTPDriver:
+ """ CTRP Driver main class
+
+ This class in inherited by all the CRTP link drivers.
+ """
+
+ def __init__(self):
+ """Driver constructor. Throw an exception if the driver is unable to
+ open the URI
+ """
+ self.needs_resending = True
+
+ def connect(self, uri, link_quality_callback, link_error_callback):
+ """Connect the driver to a specified URI
+
+ @param uri Uri of the link to open
+ @param link_quality_callback Callback to report link quality in percent
+ @param link_error_callback Callback to report errors (will result in
+ disconnection)
+ """
+
+ def send_packet(self, pk):
+ """Send a CRTP packet"""
+
+ def receive_packet(self, wait=0):
+ """Receive a CRTP packet.
+
+ @param wait The time to wait for a packet in second. -1 means forever
+
+ @return One CRTP packet or None if no packet has been received.
+ """
+
+ def get_status(self):
+ """
+ Return a status string from the interface.
+ """
+
+ def get_name(self):
+ """
+ Return a human readable name of the interface.
+ """
+
+ def scan_interface(self, address = None):
+ """
+ Scan interface for available Crazyflie quadcopters and return a list
+ with them.
+ """
+
+ def enum(self):
+ """Enumerate, and return a list, of the available link URI on this
+ system
+ """
+
+ def get_help(self):
+ """return the help message on how to form the URI for this driver
+ None means no help
+ """
+
+ def close(self):
+ """Close the link"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b3a490ee42b9334900c9ee9cb7b6c67a8c54c7f4
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.py
new file mode 100755
index 0000000000000000000000000000000000000000..70261e25ae121b328260934cef26792256bc20e1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.py
@@ -0,0 +1,148 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+CRTP packet and ports.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['CRTPPort', 'CRTPPacket']
+
+
+import struct
+
+
+class CRTPPort:
+ """
+ Lists the available ports for the CRTP.
+ """
+ CONSOLE = 0x00
+ PARAM = 0x02
+ COMMANDER = 0x03
+ MEM = 0x04
+ LOGGING = 0x05
+ STABILIZER = 0x06
+ DEBUGDRIVER = 0x0E
+ LINKCTRL = 0x0F
+ ALL = 0xFF
+
+
+class CRTPPacket(object):
+ """
+ A packet that can be sent via the CRTP.
+ """
+
+ def __init__(self, header=0, data=None):
+ """
+ Create an empty packet with default values.
+ """
+ self.size = 0
+ self._data = ""
+ # The two bits in position 3 and 4 needs to be set for legacy
+ # support of the bootloader
+ self.header = header | 0x3 << 2
+ self._port = (header & 0xF0) >> 4
+ self._channel = header & 0x03
+ if data:
+ self._set_data(data)
+
+ def _get_channel(self):
+ """Get the packet channel"""
+ return self._channel
+
+ def _set_channel(self, channel):
+ """Set the packet channel"""
+ self._channel = channel
+ self._update_header()
+
+ def _get_port(self):
+ """Get the packet port"""
+ return self._port
+
+ def _set_port(self, port):
+ """Set the packet port"""
+ self._port = port
+ self._update_header()
+
+ def get_header(self):
+ """Get the header"""
+ self._update_header()
+ return self.header
+
+ def set_header(self, port, channel):
+ """
+ Set the port and channel for this packet.
+ """
+ self._port = port
+ self.channel = channel
+ self._update_header()
+
+ def _update_header(self):
+ """Update the header with the port/channel values"""
+ # The two bits in position 3 and 4 needs to be set for legacy
+ # support of the bootloader
+ self.header = ((self._port & 0x0f) << 4 | 3 << 2 |
+ (self.channel & 0x03))
+
+ #Some python madness to access different format of the data
+ def _get_data(self):
+ """Get the packet data"""
+ return self._data
+
+ def _set_data(self, data):
+ """Set the packet data"""
+ if type(data) == str:
+ self._data = data
+ elif type(data) == list or type(data) == tuple:
+ if len(data) == 1:
+ self._data = struct.pack("B", data[0])
+ elif len(data) > 1:
+ self._data = struct.pack("B" * len(data), *data)
+ else:
+ self._data = ""
+ else:
+ raise Exception("Data shall be of str, tupple or list type")
+
+ def _get_data_l(self):
+ """Get the data in the packet as a list"""
+ return list(self._get_data_t())
+
+ def _get_data_t(self):
+ """Get the data in the packet as a tuple"""
+ return struct.unpack("B" * len(self._data), self._data)
+
+ def __str__(self):
+ """Get a string representation of the packet"""
+ return "{}:{} {}".format(self._port, self.channel, self.datat)
+
+ data = property(_get_data, _set_data)
+ datal = property(_get_data_l, _set_data)
+ datat = property(_get_data_t, _set_data)
+ datas = property(_get_data, _set_data)
+ port = property(_get_port, _set_port)
+ channel = property(_get_channel, _set_channel)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3eb90eb5ed7b7874acca4dbdf74ef08c6117a12f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..80aa46f4ceea7c27d2d8881c85234f20a102ff9d
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.py
@@ -0,0 +1,858 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Fake link driver used to debug the UI without using the Crazyflie.
+
+The operation of this driver can be controlled in two ways, either by
+connecting to different URIs or by sending messages to the DebugDriver port
+though CRTP once connected.
+
+For normal connections a console thread is also started that will send
+generated console output via CRTP.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['DebugDriver']
+
+from threading import Thread
+from .crtpdriver import CRTPDriver
+from .crtpstack import CRTPPacket, CRTPPort
+from .exceptions import WrongUriType
+import Queue
+import re
+import time
+import struct
+from datetime import datetime
+from cflib.crazyflie.log import LogTocElement
+from cflib.crazyflie.param import ParamTocElement
+import random
+import string
+import errno
+
+import logging
+logger = logging.getLogger(__name__)
+
+# This setup is used to debug raw memory logging
+memlogging = {0x01: {"min": 0, "max": 255, "mod": 1, "vartype": 1},
+ 0x02: {"min": 0, "max": 65000, "mod": 100, "vartype": 2},
+ 0x03: {"min": 0, "max": 100000, "mod": 1000, "vartype": 3},
+ 0x04: {"min":-100, "max": 100, "mod": 1, "vartype": 4},
+ 0x05: {"min":-10000, "max": 10000, "mod": 2000, "vartype": 5},
+ 0x06: {"min":-50000, "max": 50000, "mod": 1000, "vartype": 6},
+ 0x07: {"min": 0, "max": 255, "mod": 1, "vartype": 1}}
+
+class FakeMemory:
+
+ TYPE_I2C = 0
+ TYPE_1W = 1
+
+ def __init__(self, type, size, addr, data=None):
+ self.type = type
+ self.size = size
+ self.addr = addr
+ self.data = [0] * size
+ if data:
+ for i in range(len(data)):
+ self.data[i] = data[i]
+
+ def erase(self):
+ self.data = [0] * self.size
+
+class DebugDriver (CRTPDriver):
+ """ Debug driver used for debugging UI/communication without using a
+ Crazyflie"""
+ def __init__(self):
+ self.fakeLoggingThreads = []
+ self._fake_mems = []
+ # Fill up the fake logging TOC with values and data
+ self.fakeLogToc = []
+ self.fakeLogToc.append({"varid": 0, "vartype": 5, "vargroup": "imu",
+ "varname": "gyro_x", "min":-10000,
+ "max": 10000, "mod": 1000})
+ self.fakeLogToc.append({"varid": 1, "vartype": 5, "vargroup": "imu",
+ "varname": "gyro_y", "min":-10000,
+ "max": 10000, "mod": 150})
+ self.fakeLogToc.append({"varid": 2, "vartype": 5, "vargroup": "imu",
+ "varname": "gyro_z", "min":-10000,
+ "max": 10000, "mod": 200})
+ self.fakeLogToc.append({"varid": 3, "vartype": 5, "vargroup": "imu",
+ "varname": "acc_x", "min":-1000,
+ "max": 1000, "mod": 15})
+ self.fakeLogToc.append({"varid": 4, "vartype": 5, "vargroup": "imu",
+ "varname": "acc_y", "min":-1000,
+ "max": 1000, "mod": 10})
+ self.fakeLogToc.append({"varid": 5, "vartype": 5, "vargroup": "imu",
+ "varname": "acc_z", "min":-1000,
+ "max": 1000, "mod": 20})
+ self.fakeLogToc.append({"varid": 6, "vartype": 7,
+ "vargroup": "stabilizer", "varname": "roll",
+ "min":-90, "max": 90, "mod": 2})
+ self.fakeLogToc.append({"varid": 7, "vartype": 7,
+ "vargroup": "stabilizer", "varname": "pitch",
+ "min":-90, "max": 90, "mod": 1.5})
+ self.fakeLogToc.append({"varid": 8, "vartype": 7,
+ "vargroup": "stabilizer", "varname": "yaw",
+ "min":-90, "max": 90, "mod": 2.5})
+ self.fakeLogToc.append({"varid": 9, "vartype": 7, "vargroup": "pm",
+ "varname": "vbat", "min": 3.0,
+ "max": 4.2, "mod": 0.1})
+ self.fakeLogToc.append({"varid": 10, "vartype": 6, "vargroup": "motor",
+ "varname": "m1", "min": 0, "max": 65000,
+ "mod": 1000})
+ self.fakeLogToc.append({"varid": 11, "vartype": 6, "vargroup": "motor",
+ "varname": "m2", "min": 0, "max": 65000,
+ "mod": 1000})
+ self.fakeLogToc.append({"varid": 12, "vartype": 6, "vargroup": "motor",
+ "varname": "m3", "min": 0, "max": 65000,
+ "mod": 1000})
+ self.fakeLogToc.append({"varid": 13, "vartype": 6, "vargroup": "motor",
+ "varname": "m4", "min": 0, "max": 65000,
+ "mod": 1000})
+ self.fakeLogToc.append({"varid": 14, "vartype": 2,
+ "vargroup": "stabilizer", "varname": "thrust",
+ "min": 0, "max": 65000, "mod": 1000})
+ self.fakeLogToc.append({"varid": 15, "vartype": 7,
+ "vargroup": "baro", "varname": "asl",
+ "min": 540, "max": 545, "mod": 0.5})
+ self.fakeLogToc.append({"varid": 16, "vartype": 7,
+ "vargroup": "baro", "varname": "aslRaw",
+ "min": 540, "max": 545, "mod": 1.0})
+ self.fakeLogToc.append({"varid": 17, "vartype": 7,
+ "vargroup": "baro", "varname": "aslLong",
+ "min": 540, "max": 545, "mod": 0.5})
+ self.fakeLogToc.append({"varid": 18, "vartype": 7,
+ "vargroup": "baro", "varname": "temp",
+ "min": 26, "max": 38, "mod": 1.0})
+ self.fakeLogToc.append({"varid": 19, "vartype": 7,
+ "vargroup": "altHold", "varname": "target",
+ "min": 542, "max": 543, "mod": 0.1})
+ self.fakeLogToc.append({"varid": 20, "vartype": 6,
+ "vargroup": "gps", "varname": "lat",
+ "min": 556112190, "max": 556112790,
+ "mod": 10})
+ self.fakeLogToc.append({"varid": 21, "vartype": 6,
+ "vargroup": "gps", "varname": "lon",
+ "min": 129945110, "max": 129945710,
+ "mod": 10})
+ self.fakeLogToc.append({"varid": 22, "vartype": 6,
+ "vargroup": "gps", "varname": "hMSL",
+ "min": 0, "max": 100000,
+ "mod": 1000})
+ self.fakeLogToc.append({"varid": 23, "vartype": 6,
+ "vargroup": "gps", "varname": "heading",
+ "min": -10000000, "max": 10000000,
+ "mod": 100000})
+ self.fakeLogToc.append({"varid": 24, "vartype": 6,
+ "vargroup": "gps", "varname": "gSpeed",
+ "min": 0, "max": 1000,
+ "mod": 100})
+ self.fakeLogToc.append({"varid": 25, "vartype": 3,
+ "vargroup": "gps", "varname": "hAcc",
+ "min": 0, "max": 5000,
+ "mod": 100})
+ self.fakeLogToc.append({"varid": 26, "vartype": 1,
+ "vargroup": "gps", "varname": "fixType",
+ "min": 0, "max": 5,
+ "mod": 1})
+
+
+ # Fill up the fake logging TOC with values and data
+ self.fakeParamToc = []
+ self.fakeParamToc.append({"varid": 0, "vartype": 0x08,
+ "vargroup": "blah", "varname": "p",
+ "writable": True, "value": 100})
+ self.fakeParamToc.append({"varid": 1, "vartype": 0x0A,
+ "vargroup": "info", "varname": "cid",
+ "writable": False, "value": 1234})
+ self.fakeParamToc.append({"varid": 2, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "prp",
+ "writable": True, "value": 1.5})
+ self.fakeParamToc.append({"varid": 3, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "pyaw",
+ "writable": True, "value": 2.5})
+ self.fakeParamToc.append({"varid": 4, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "irp",
+ "writable": True, "value": 3.5})
+ self.fakeParamToc.append({"varid": 5, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "iyaw",
+ "writable": True, "value": 4.5})
+ self.fakeParamToc.append({"varid": 6, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "drp",
+ "writable": True, "value": 5.5})
+ self.fakeParamToc.append({"varid": 7, "vartype": 0x06,
+ "vargroup": "rpid", "varname": "dyaw",
+ "writable": True, "value": 6.5})
+ self.fakeParamToc.append({"varid": 8, "vartype": 0x06,
+ "vargroup": "apid", "varname": "prp",
+ "writable": True, "value": 7.5})
+ self.fakeParamToc.append({"varid": 9, "vartype": 0x06,
+ "vargroup": "apid", "varname": "pyaw",
+ "writable": True, "value": 8.5})
+ self.fakeParamToc.append({"varid": 10, "vartype": 0x06,
+ "vargroup": "apid", "varname": "irp",
+ "writable": True, "value": 9.5})
+ self.fakeParamToc.append({"varid": 11, "vartype": 0x06,
+ "vargroup": "apid", "varname": "iyaw",
+ "writable": True, "value": 10.5})
+ self.fakeParamToc.append({"varid": 12, "vartype": 0x06,
+ "vargroup": "apid", "varname": "drp",
+ "writable": True, "value": 11.5})
+ self.fakeParamToc.append({"varid": 13, "vartype": 0x06,
+ "vargroup": "apid", "varname": "dyaw",
+ "writable": True, "value": 12.5})
+ self.fakeParamToc.append({"varid": 14, "vartype": 0x08,
+ "vargroup": "flightctrl",
+ "varname": "xmode", "writable": True,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 15, "vartype": 0x08,
+ "vargroup": "flightctrl",
+ "varname": "ratepid", "writable": True,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 16, "vartype": 0x08,
+ "vargroup": "imu_sensors",
+ "varname": "HMC5883L", "writable": False,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 17, "vartype": 0x08,
+ "vargroup": "imu_sensors",
+ "varname": "MS5611", "writable": False,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 18, "vartype": 0x0A,
+ "vargroup": "firmware",
+ "varname": "revision0", "writable": False,
+ "value": 0xdeb})
+ self.fakeParamToc.append({"varid": 19, "vartype": 0x09,
+ "vargroup": "firmware",
+ "varname": "revision1", "writable": False,
+ "value": 0x99})
+ self.fakeParamToc.append({"varid": 20, "vartype": 0x08,
+ "vargroup": "firmware",
+ "varname": "modified", "writable": False,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 21, "vartype": 0x08,
+ "vargroup": "imu_tests",
+ "varname": "MPU6050", "writable": False,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 22, "vartype": 0x08,
+ "vargroup": "imu_tests",
+ "varname": "HMC5883L", "writable": False,
+ "value": 1})
+ self.fakeParamToc.append({"varid": 23, "vartype": 0x08,
+ "vargroup": "imu_tests",
+ "varname": "MS5611", "writable": False,
+ "value": 1})
+
+ self.fakeflash = {}
+ self._random_answer_delay = True
+ self.queue = Queue.Queue()
+ self._packet_handler = _PacketHandlingThread(self.queue,
+ self.fakeLogToc,
+ self.fakeParamToc,
+ self._fake_mems)
+ self._packet_handler.start()
+
+ def scan_interface(self, address):
+ return [["debug://0/0", "Normal connection"],
+ ["debug://0/1", "Fail to connect"],
+ ["debug://0/2", "Incomplete log TOC download"],
+ ["debug://0/3", "Insert random delays on replies"],
+ ["debug://0/4", "Insert random delays on replies and random TOC CRCs"],
+ ["debug://0/5", "Normal but random TOC CRCs"],
+ ["debug://0/6", "Normal but empty I2C and OW mems"]]
+
+
+ def get_status(self):
+ return "Ok"
+
+ def get_name(self):
+ return "debug"
+
+ def connect(self, uri, linkQualityCallback, linkErrorCallback):
+
+ if not re.search("^debug://", uri):
+ raise WrongUriType("Not a debug URI")
+
+ self._packet_handler.linkErrorCallback = linkErrorCallback
+ self._packet_handler.linkQualityCallback = linkQualityCallback
+
+ # Debug-options for this driver that
+ # is set by using different connection URIs
+ self._packet_handler.inhibitAnswers = False
+ self._packet_handler.doIncompleteLogTOC = False
+ self._packet_handler.bootloader = False
+ self._packet_handler._random_answer_delay = False
+ self._packet_handler._random_toc_crcs = False
+
+ if (re.search("^debug://.*/1\Z", uri)):
+ self._packet_handler.inhibitAnswers = True
+ if (re.search("^debug://.*/110\Z", uri)):
+ self._packet_handler.bootloader = True
+ if (re.search("^debug://.*/2\Z", uri)):
+ self._packet_handler.doIncompleteLogTOC = True
+ if (re.search("^debug://.*/3\Z", uri)):
+ self._packet_handler._random_answer_delay = True
+ if (re.search("^debug://.*/4\Z", uri)):
+ self._packet_handler._random_answer_delay = True
+ self._packet_handler._random_toc_crcs = True
+ if (re.search("^debug://.*/5\Z", uri)):
+ self._packet_handler._random_toc_crcs = True
+
+ if len(self._fake_mems) == 0:
+ # Insert some data here
+ self._fake_mems.append(FakeMemory(type=0, size=100, addr=0))
+ self._fake_mems.append(FakeMemory(type=1, size=112, addr=0x1234567890ABCDEF,
+ data=[0xeb, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x44, 0x00, 0x0e,
+ 0x01, 0x09, 0x62, 0x63, 0x4c, 0x65, 0x64, 0x52, 0x69, 0x6e,
+ 0x67, 0x02, 0x01, 0x62, 0x55]))
+ #self._fake_mems.append(FakeMemory(type=1, size=112, addr=0xFEDCBA0987654321,
+ # data=[0xeb, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x44, 0x00, 0x44,
+ # 0x01, 0x2e, 0x54, 0x68, 0x69, 0x73, 0x20, 0x69, 0x73, 0x20,
+ # 0x61, 0x20, 0x72, 0x65, 0x61, 0x6c, 0x6c, 0x79, 0x20, 0x6c,
+ # 0x6f, 0x6e, 0x67, 0x20, 0x6e, 0x61, 0x6d, 0x65, 0x2c, 0x20,
+ # 0x74, 0x68, 0x61, 0x74, 0x27, 0x73, 0x20, 0x6a, 0x75, 0x73,
+ # 0x74, 0x20, 0x67, 0x72, 0x65, 0x61, 0x74, 0x21, 0x02, 0x12,
+ # 0x53, 0x75, 0x70, 0x65, 0x72, 0x20, 0x72, 0x65, 0x76, 0x69,
+ # 0x73, 0x69, 0x6f, 0x6e, 0x20, 0x41, 0x35, 0x35, 0xc8]))
+
+ if (re.search("^debug://.*/6\Z", uri)):
+ logger.info("------------->Erasing memories on connect")
+ for m in self._fake_mems:
+ m.erase()
+
+ self.fakeConsoleThread = None
+
+ if (not self._packet_handler.inhibitAnswers and not self._packet_handler.bootloader):
+ self.fakeConsoleThread = FakeConsoleThread(self.queue)
+ self.fakeConsoleThread.start()
+
+ if (self._packet_handler.linkQualityCallback is not None):
+ self._packet_handler.linkQualityCallback(0)
+
+ def receive_packet(self, time=0):
+ if time == 0:
+ try:
+ return self.queue.get(False)
+ except Queue.Empty:
+ return None
+ elif time < 0:
+ try:
+ return self.queue.get(True)
+ except Queue.Empty:
+ return None
+ else:
+ try:
+ return self.queue.get(True, time)
+ except Queue.Empty:
+ return None
+
+ def send_packet(self, pk):
+ self._packet_handler.handle_packet(pk)
+
+ def close(self):
+ logger.info("Closing debugdriver")
+ for f in self._packet_handler.fakeLoggingThreads:
+ f.stop()
+ if self.fakeConsoleThread:
+ self.fakeConsoleThread.stop()
+
+
+class _PacketHandlingThread(Thread):
+ """Thread for handling packets asynchronously"""
+ def __init__(self, out_queue, fake_log_toc, fake_param_toc, fake_mems):
+ Thread.__init__(self)
+ self.setDaemon(True)
+ self.queue = out_queue
+ self.fakeLogToc = fake_log_toc
+ self.fakeParamToc = fake_param_toc
+ self._fake_mems = fake_mems
+ self._in_queue = Queue.Queue()
+
+ self.inhibitAnswers = False
+ self.doIncompleteLogTOC = False
+ self.bootloader = False
+ self._random_answer_delay = False
+ self._random_toc_crcs = False
+
+ self.linkErrorCallback = None
+ self.linkQualityCallback = None
+ random.seed(None)
+ self.fakeLoggingThreads = []
+
+ self._added_blocks = []
+
+ self.nowAnswerCounter = 4
+
+ def handle_packet(self, pk):
+ self._in_queue.put(pk)
+
+ def run(self):
+ while (True):
+ pk = self._in_queue.get(True)
+ if (self.inhibitAnswers):
+ self.nowAnswerCounter = self.nowAnswerCounter - 1
+ logger.debug("Not answering with any data, will send link errori"
+ " in %d retries", self.nowAnswerCounter)
+ if (self.nowAnswerCounter == 0):
+ self.linkErrorCallback("Nothing is answering, and it"
+ " shouldn't")
+ else:
+ if (pk.port == 0xFF):
+ self._handle_bootloader(pk)
+ elif (pk.port == CRTPPort.DEBUGDRIVER):
+ self._handle_debugmessage(pk)
+ elif (pk.port == CRTPPort.COMMANDER):
+ pass
+ elif (pk.port == CRTPPort.LOGGING):
+ self._handle_logging(pk)
+ elif (pk.port == CRTPPort.PARAM):
+ self.handleParam(pk)
+ elif (pk.port == CRTPPort.MEM):
+ self._handle_mem_access(pk)
+ else:
+ logger.warning("Not handling incoming packets on port [%d]",
+ pk.port)
+
+ def _handle_mem_access(self, pk):
+ chan = pk.channel
+ cmd = struct.unpack("B", pk.data[0])[0]
+ payload = struct.pack("B" * (len(pk.datal) - 1), *pk.datal[1:])
+
+ if chan == 0: # Info channel
+ p_out = CRTPPacket()
+ p_out.set_header(CRTPPort.MEM, 0)
+ if cmd == 1: # Request number of memories
+ p_out.data = (1, len(self._fake_mems))
+ if cmd == 2:
+ id = ord(payload[0])
+ logger.info("Getting mem {}".format(id))
+ m = self._fake_mems[id]
+ p_out.data = struct.pack('<BBBIQ', 2, id, m.type, m.size, m.addr)
+ self._send_packet(p_out)
+
+ if chan == 1: # Read channel
+ id = cmd
+ addr = struct.unpack("I", payload[0:4])[0]
+ length = ord(payload[4])
+ status = 0
+ logger.info("MEM: Read {}bytes at 0x{:X} from memory {}".format(length, addr, id))
+ m = self._fake_mems[id]
+ p_out = CRTPPacket()
+ p_out.set_header(CRTPPort.MEM, 1)
+ p_out.data = struct.pack("<BIB", id, addr, status)
+ p_out.data += struct.pack("B"*length, *m.data[addr:addr+length])
+ self._send_packet(p_out)
+
+ if chan == 2: # Write channel
+ id = cmd
+ addr = struct.unpack("I", payload[0:4])[0]
+ data = payload[4:]
+ logger.info("MEM: Write {}bytes at 0x{:X} to memory {}".format(len(data), addr, id))
+ m = self._fake_mems[id]
+
+ for i in range(len(data)):
+ m.data[addr+i] = ord(data[i])
+
+ status = 0
+
+ p_out = CRTPPacket()
+ p_out.set_header(CRTPPort.MEM, 2)
+ p_out.data = struct.pack("<BIB", id, addr, status)
+ self._send_packet(p_out)
+
+ def _handle_bootloader(self, pk):
+ cmd = pk.datal[1]
+ if (cmd == 0x10): # Request info about copter
+ p = CRTPPacket()
+ p.set_header(0xFF, 0xFF)
+ pageSize = 1024
+ buffPages = 10
+ flashPages = 100
+ flashStart = 1
+ p.data = struct.pack('<BBHHHH', 0xFF, 0x10, pageSize, buffPages,
+ flashPages, flashStart)
+ p.data += struct.pack('B' * 12, 0xA0A1A2A3A4A5)
+ self._send_packet(p)
+ logging.info("Bootloader: Sending info back info")
+ elif (cmd == 0x14): # Upload buffer
+ [page, addr] = struct.unpack('<HH', p.data[0:4])
+ elif (cmd == 0x18): # Flash page
+ p = CRTPPacket()
+ p.set_header(0xFF, 0xFF)
+ p.data = struct.pack('<BBH', 0xFF, 0x18, 1)
+ self._send_packet(p)
+ elif (cmd == 0xFF): # Reset to firmware
+ logger.info("Bootloader: Got reset command")
+ else:
+ logger.warning("Bootloader: Unknown command 0x%02X", cmd)
+
+ def _handle_debugmessage(self, pk):
+ if (pk.channel == 0):
+ cmd = struct.unpack("B", pk.data[0])[0]
+ if (cmd == 0): # Fake link quality
+ newLinkQuality = struct.unpack("B", pk.data[1])[0]
+ self.linkQualityCallback(newLinkQuality)
+ elif (cmd == 1):
+ self.linkErrorCallback("DebugDriver was forced to disconnect!")
+ else:
+ logger.warning("Debug port: Not handling cmd=%d on channel 0",
+ cmd)
+ else:
+ logger.warning("Debug port: Not handling channel=%d",
+ pk.channel)
+
+ def _handle_toc_access(self, pk):
+ chan = pk.channel
+ cmd = struct.unpack("B", pk.data[0])[0]
+ logger.info("TOC access on port %d", pk.port)
+ if (chan == 0): # TOC Access
+ cmd = struct.unpack("B", pk.data[0])[0]
+ if (cmd == 0): # Reqest variable info
+ p = CRTPPacket()
+ p.set_header(pk.port, 0)
+ varIndex = 0
+ if (len(pk.data) > 1):
+ varIndex = struct.unpack("B", pk.data[1])[0]
+ logger.debug("TOC[%d]: Requesting ID=%d", pk.port,
+ varIndex)
+ else:
+ logger.debug("TOC[%d]: Requesting first index..surprise,"
+ " it 0 !", pk.port)
+
+ if (pk.port == CRTPPort.LOGGING):
+ l = self.fakeLogToc[varIndex]
+ if (pk.port == CRTPPort.PARAM):
+ l = self.fakeParamToc[varIndex]
+
+ vartype = l["vartype"]
+ if (pk.port == CRTPPort.PARAM and l["writable"] is True):
+ vartype = vartype | (0x10)
+
+ p.data = struct.pack("<BBB", cmd, l["varid"], vartype)
+ for ch in l["vargroup"]:
+ p.data += ch
+ p.data += '\0'
+ for ch in l["varname"]:
+ p.data += ch
+ p.data += '\0'
+ if (self.doIncompleteLogTOC is False):
+ self._send_packet(p)
+ elif (varIndex < 5):
+ self._send_packet(p)
+ else:
+ logger.info("TOC: Doing incomplete TOC, stopping after"
+ " varIndex => 5")
+
+ if (cmd == 1): # TOC CRC32 request
+ fakecrc = 0
+ if (pk.port == CRTPPort.LOGGING):
+ tocLen = len(self.fakeLogToc)
+ fakecrc = 0xAAAAAAAA
+ if (pk.port == CRTPPort.PARAM):
+ tocLen = len(self.fakeParamToc)
+ fakecrc = 0xBBBBBBBB
+
+ if self._random_toc_crcs:
+ fakecrc = int(''.join(random.choice("ABCDEF" + string.digits) for x in range(8)), 16)
+ logger.debug("Generated random TOC CRC: 0x%x", fakecrc)
+ logger.info("TOC[%d]: Requesting TOC CRC, sending back fake"
+ " stuff: %d", pk.port, len(self.fakeLogToc))
+ p = CRTPPacket()
+ p.set_header(pk.port, 0)
+ p.data = struct.pack('<BBIBB', 1, tocLen, fakecrc, 16, 24)
+ self._send_packet(p)
+
+ def handleParam(self, pk):
+ chan = pk.channel
+ cmd = struct.unpack("B", pk.data[0])[0]
+ logger.debug("PARAM: Port=%d, Chan=%d, cmd=%d", pk.port,
+ chan, cmd)
+ if (chan == 0): # TOC Access
+ self._handle_toc_access(pk)
+ elif (chan == 2): # Settings access
+ varId = pk.datal[0]
+ formatStr = ParamTocElement.types[self.fakeParamToc
+ [varId]["vartype"]][1]
+ newvalue = struct.unpack(formatStr, pk.data[1:])[0]
+ self.fakeParamToc[varId]["value"] = newvalue
+ logger.info("PARAM: New value [%s] for param [%d]", newvalue,
+ varId)
+ # Send back the new value
+ p = CRTPPacket()
+ p.set_header(pk.port, 2)
+ p.data += struct.pack("<B", varId)
+ p.data += struct.pack(formatStr, self.fakeParamToc[varId]["value"])
+ self._send_packet(p)
+ elif (chan == 1):
+ p = CRTPPacket()
+ p.set_header(pk.port, 1)
+ varId = cmd
+ p.data += struct.pack("<B", varId)
+ formatStr = ParamTocElement.types[self.fakeParamToc
+ [varId]["vartype"]][1]
+ p.data += struct.pack(formatStr, self.fakeParamToc[varId]["value"])
+ logger.info("PARAM: Getting value for %d", varId)
+ self._send_packet(p)
+
+ def _handle_logging(self, pk):
+ chan = pk.channel
+ cmd = struct.unpack("B", pk.data[0])[0]
+ logger.debug("LOG: Chan=%d, cmd=%d", chan, cmd)
+ if (chan == 0): # TOC Access
+ self._handle_toc_access(pk)
+ elif (chan == 1): # Settings access
+ if (cmd == 0):
+ blockId = ord(pk.data[1])
+ if blockId not in self._added_blocks:
+ self._added_blocks.append(blockId)
+ logger.info("LOG:Adding block id=%d", blockId)
+ listofvars = pk.data[3:]
+ fakeThread = _FakeLoggingDataThread(self.queue, blockId,
+ listofvars,
+ self.fakeLogToc)
+ self.fakeLoggingThreads.append(fakeThread)
+ fakeThread.start()
+ # Anser that everything is ok
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', 0, blockId, 0x00)
+ self._send_packet(p)
+ else:
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', 0, blockId, errno.EEXIST)
+ self._send_packet(p)
+ if (cmd == 1):
+ logger.warning("LOG: Appending block not implemented!")
+ if (cmd == 2):
+ blockId = ord(pk.data[1])
+ logger.info("LOG: Should delete block %d", blockId)
+ success = False
+ for fb in self.fakeLoggingThreads:
+ if (fb.blockId == blockId):
+ fb._disable_logging()
+ fb.stop()
+
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', cmd, blockId, 0x00)
+ self._send_packet(p)
+ logger.info("LOG: Deleted block=%d", blockId)
+ success = True
+ if (success is False):
+ logger.warning("LOG: Could not delete block=%d, not found",
+ blockId)
+ # TODO: Send back error code
+
+ if (cmd == 3):
+ blockId = ord(pk.data[1])
+ period = ord(pk.data[2]) * 10 # Sent as multiple of 10 ms
+ logger.info("LOG:Starting block %d", blockId)
+ success = False
+ for fb in self.fakeLoggingThreads:
+ if (fb.blockId == blockId):
+ fb._enable_logging()
+ fb.period = period
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', cmd, blockId, 0x00)
+ self._send_packet(p)
+ logger.info("LOG:Started block=%d", blockId)
+ success = True
+ if (success is False):
+ logger.info("LOG:Could not start block=%d, not found",
+ blockId)
+ # TODO: Send back error code
+ if (cmd == 4):
+ blockId = ord(pk.data[1])
+ logger.info("LOG:Pausing block %d", blockId)
+ success = False
+ for fb in self.fakeLoggingThreads:
+ if (fb.blockId == blockId):
+ fb._disable_logging()
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', cmd, blockId, 0x00)
+ self._send_packet(p)
+ logger.info("LOG:Pause block=%d", blockId)
+ success = True
+ if (success is False):
+ logger.warning("LOG:Could not pause block=%d, not found",
+ blockId)
+ # TODO: Send back error code
+ if (cmd == 5):
+ logger.info("LOG: Reset logging, but doing nothing")
+ p = CRTPPacket()
+ p.set_header(5, 1)
+ p.data = struct.pack('<BBB', cmd, 0x00, 0x00)
+ self._send_packet(p)
+ import traceback
+ logger.info(traceback.format_exc())
+ elif (chan > 1):
+ logger.warning("LOG: Uplink packets with channels > 1 not"
+ " supported!")
+
+ def _send_packet(self, pk):
+ # Do not delay log data
+ if self._random_answer_delay and pk.port != 0x05 and pk.channel != 0x02:
+ # Calculate a delay between 0ms and 250ms
+ delay = random.randint(0, 250)/1000.0
+ logger.debug("Delaying answer %.2fms", delay*1000)
+ time.sleep(delay)
+ self.queue.put(pk)
+
+class _FakeLoggingDataThread (Thread):
+ """Thread that will send back fake logging data via CRTP"""
+
+ def __init__(self, outQueue, blockId, listofvars, fakeLogToc):
+ Thread.__init__(self)
+ self.starttime = datetime.now()
+ self.outQueue = outQueue
+ self.setDaemon(True)
+ self.mod = 0
+ self.blockId = blockId
+ self.period = 0
+ self.listofvars = listofvars
+ self.shouldLog = False
+ self.fakeLogToc = fakeLogToc
+ self.fakeLoggingData = []
+ self.setName("Fakelog block=%d" % blockId)
+ self.shouldQuit = False
+
+ logging.info("FakeDataLoggingThread created for blockid=%d", blockId)
+ i = 0
+ while (i < len(listofvars)):
+ varType = ord(listofvars[i])
+ var_stored_as = (varType >> 8)
+ var_fetch_as = (varType & 0xFF)
+ if (var_stored_as > 0):
+ addr = struct.unpack("<I", listofvars[i + 1:i + 5])
+ logger.debug("FakeLoggingThread: We should log a memory addr"
+ " 0x%04X", addr)
+ self.fakeLoggingData.append([memlogging[var_fetch_as],
+ memlogging[var_fetch_as]["min"],
+ 1])
+ i = i + 5
+ else:
+ varId = ord(listofvars[i])
+ logger.debug("FakeLoggingThread: We should log variable from"
+ " TOC: id=%d, type=0x%02X", varId, varType)
+ for t in self.fakeLogToc:
+ if (varId == t["varid"]):
+ # Each touple will have var data and current fake value
+ self.fakeLoggingData.append([t, t["min"], 1])
+ i = i + 2
+
+ def _enable_logging(self):
+ self.shouldLog = True
+ logging.info("_FakeLoggingDataThread: Enable thread [%s] at period %d",
+ self.getName(), self.period)
+
+ def _disable_logging(self):
+ self.shouldLog = False
+ logging.info("_FakeLoggingDataThread: Disable thread [%s]",
+ self.getName())
+
+ def stop(self):
+ self.shouldQuit = True
+
+ def run(self):
+ while(self.shouldQuit is False):
+ if (self.shouldLog is True):
+
+ p = CRTPPacket()
+ p.set_header(5, 2)
+ p.data = struct.pack('<B', self.blockId)
+ timestamp = int((datetime.now()-self.starttime).total_seconds()*1000)
+ p.data += struct.pack('BBB', timestamp&0xff, (timestamp>>8)&0x0ff, (timestamp>>16)&0x0ff) # Timestamp
+
+ for d in self.fakeLoggingData:
+ # Set new value
+ d[1] = d[1] + d[0]["mod"] * d[2]
+ # Obej the limitations
+ if (d[1] > d[0]["max"]):
+ d[1] = d[0]["max"] # Limit value
+ d[2] = -1 # Switch direction
+ if (d[1] < d[0]["min"]):
+ d[1] = d[0]["min"] # Limit value
+ d[2] = 1 # Switch direction
+ # Pack value
+ formatStr = LogTocElement.types[d[0]["vartype"]][1]
+ p.data += struct.pack(formatStr, d[1])
+ self.outQueue.put(p)
+ time.sleep(self.period / 1000.0) # Period in ms here
+
+
+class FakeConsoleThread (Thread):
+ """Thread that will send back fake console data via CRTP"""
+ def __init__(self, outQueue):
+ Thread.__init__(self)
+ self.outQueue = outQueue
+ self.setDaemon(True)
+ self._should_run = True
+
+ def stop(self):
+ self._shoud_run = False
+
+ def run(self):
+ # Temporary hack to test GPS from firmware by sending NMEA string on console
+ long_val = 0
+ lat_val = 0
+ alt_val = 0
+
+ while(self._should_run):
+
+ long_val += 1
+ lat_val += 1
+ alt_val += 1.0
+
+ long_string = "5536.677%d" % (long_val % 99)
+ lat_string = "01259.645%d" % (lat_val % 99)
+ alt_string = "%.1f" % (alt_val % 100.0)
+
+ # Copy of what is sent from the module, but note that only
+ # the GPGGA message is being simulated, the others are fixed...
+ self._send_text("Time is now %s\n" % datetime.now())
+ self._send_text("$GPVTG,,T,,M,0.386,N,0.716,K,A*2E\n")
+ self._send_text("$GPGGA,135544.0")
+ self._send_text("0,%s,N,%s,E,1,04,2.62,3.6,M,%s,M,,*58\n" % (long_string, lat_string, alt_string))
+ self._send_text("$GPGSA,A,3,31,20,23,07,,,,,,,,,3.02,2.62,1.52*05\n")
+ self._send_text("$GPGSV,2,1,07,07,09,181,15,13,63,219,26,16,02,097,,17,05,233,20*7E\n")
+ self._send_text("$GPGSV,2,2,07,20,42,119,35,23,77,097,27,31,12,032,19*47\n")
+ self._send_text("$GPGLL,5536.67734,N,01259.64578,E,135544.00,A,A*68\n")
+
+ time.sleep(2)
+
+ def _send_text(self, message):
+ p = CRTPPacket()
+ p.set_header(0, 0)
+
+ us = "%is" % len(message)
+ # This might be done prettier ;-)
+ p.data = struct.pack(us, message)
+
+ self.outQueue.put(p)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f28249048fede5906ec54557b98d959df95754e6
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.py
new file mode 100755
index 0000000000000000000000000000000000000000..cb286caca7b01dafe566a755c9e4b32b65969885
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.py
@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Exception used when the URI is not for the current driver
+(ie. radio:// for the serial driver ...)
+It basically means that an other driver could do the job
+It does NOT means that the URI is good or bad
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['WrongUriType', 'CommunicationException']
+
+
+class WrongUriType (Exception):
+ """ Wrong type of URI for this interface """
+ pass
+
+
+class CommunicationException (Exception):
+ """ Communication problem when communicating with a Crazyflie """
+ pass
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3eafb262e3d3ac3a49949b7b8635706859b222b8
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..1b00698b98967bec51ec22de106529097a828aee
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py
@@ -0,0 +1,424 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Crazyradio CRTP link driver.
+
+This driver is used to communicate with the Crazyflie using the Crazyradio
+USB dongle.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['RadioDriver']
+
+import logging
+logger = logging.getLogger(__name__)
+
+from cflib.crtp.crtpdriver import CRTPDriver
+from .crtpstack import CRTPPacket
+from .exceptions import WrongUriType
+import threading
+import Queue
+import re
+import array
+import binascii
+import struct
+
+from cflib.drivers.crazyradio import Crazyradio
+from usb import USBError
+
+
+class RadioDriver(CRTPDriver):
+ """ Crazyradio link driver """
+ def __init__(self):
+ """ Create the link driver """
+ CRTPDriver.__init__(self)
+ self.cradio = None
+ self.uri = ""
+ self.link_error_callback = None
+ self.link_quality_callback = None
+ self.in_queue = None
+ self.out_queue = None
+ self._thread = None
+
+ def connect(self, uri, link_quality_callback, link_error_callback):
+ """
+ Connect the link driver to a specified URI of the format:
+ radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
+
+ The callback for linkQuality can be called at any moment from the
+ driver to report back the link quality in percentage. The
+ callback from linkError will be called when a error occurs with
+ an error message.
+ """
+
+ # check if the URI is a radio URI
+ if not re.search("^radio://", uri):
+ raise WrongUriType("Not a radio URI")
+
+ # Open the USB dongle
+ if not re.search("^radio://([0-9]+)((/([0-9]+))((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$",
+ uri):
+ raise WrongUriType('Wrong radio URI format!')
+
+ uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
+ "((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$",
+ uri)
+
+ self.uri = uri
+
+ channel = 2
+ if uri_data.group(4):
+ channel = int(uri_data.group(4))
+
+ datarate = Crazyradio.DR_2MPS
+ if uri_data.group(7) == "250K":
+ datarate = Crazyradio.DR_250KPS
+ if uri_data.group(7) == "1M":
+ datarate = Crazyradio.DR_1MPS
+ if uri_data.group(7) == "2M":
+ datarate = Crazyradio.DR_2MPS
+
+ if self.cradio is None:
+ self.cradio = Crazyradio(devid=int(uri_data.group(1)))
+ else:
+ raise Exception("Link already open!")
+
+ if self.cradio.version >= 0.4:
+ self.cradio.set_arc(10)
+ else:
+ logger.warning("Radio version <0.4 will be obsoleted soon!")
+
+ self.cradio.set_channel(channel)
+
+ self.cradio.set_data_rate(datarate)
+
+ if uri_data.group(9):
+ addr = str(uri_data.group(9))
+ new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
+ self.cradio.set_address(new_addr)
+
+ # Prepare the inter-thread communication queue
+ self.in_queue = Queue.Queue()
+ # Limited size out queue to avoid "ReadBack" effect
+ self.out_queue = Queue.Queue(1)
+
+ # Launch the comm thread
+ self._thread = _RadioDriverThread(self.cradio, self.in_queue,
+ self.out_queue,
+ link_quality_callback,
+ link_error_callback)
+ self._thread.start()
+
+ self.link_error_callback = link_error_callback
+
+ def receive_packet(self, time=0):
+ """
+ Receive a packet though the link. This call is blocking but will
+ timeout and return None if a timeout is supplied.
+ """
+ if time == 0:
+ try:
+ return self.in_queue.get(False)
+ except Queue.Empty:
+ return None
+ elif time < 0:
+ try:
+ return self.in_queue.get(True)
+ except Queue.Empty:
+ return None
+ else:
+ try:
+ return self.in_queue.get(True, time)
+ except Queue.Empty:
+ return None
+
+ def send_packet(self, pk):
+ """ Send the packet pk though the link """
+ # if self.out_queue.full():
+ # self.out_queue.get()
+ if (self.cradio is None):
+ return
+
+ try:
+ self.out_queue.put(pk, True, 2)
+ except Queue.Full:
+ if self.link_error_callback:
+ self.link_error_callback("RadioDriver: Could not send packet"
+ " to copter")
+
+ def pause(self):
+ self._thread.stop()
+ self._thread = None
+
+ def restart(self):
+ if self._thread:
+ return
+
+ self._thread = _RadioDriverThread(self.cradio, self.in_queue,
+ self.out_queue,
+ self.link_quality_callback,
+ self.link_error_callback)
+ self._thread.start()
+
+ def close(self):
+ """ Close the link. """
+ # Stop the comm thread
+ self._thread.stop()
+
+ # Close the USB dongle
+ try:
+ if self.cradio:
+ self.cradio.close()
+ except:
+ # If we pull out the dongle we will not make this call
+ pass
+ self.cradio = None
+
+ # Clear callbacks
+ self.link_error_callback = None
+ self.link_quality_callback = None
+
+ def _scan_radio_channels(self, start=0, stop=125):
+ """ Scan for Crazyflies between the supplied channels. """
+ return list(self.cradio.scan_channels(start, stop, (0xff,)))
+
+ def scan_selected(self, links):
+ to_scan = ()
+ for l in links:
+ one_to_scan = {}
+ uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
+ "(/(250K|1M|2M))?)?$",
+ l)
+
+ one_to_scan["channel"] = int(uri_data.group(4))
+
+ datarate = Crazyradio.DR_2MPS
+ if uri_data.group(6) == "250K":
+ datarate = Crazyradio.DR_250KPS
+ if uri_data.group(6) == "1M":
+ datarate = Crazyradio.DR_1MPS
+ if uri_data.group(6) == "2M":
+ datarate = Crazyradio.DR_2MPS
+
+ one_to_scan["datarate"] = datarate
+
+ to_scan += (one_to_scan, )
+
+ found = self.cradio.scan_selected(to_scan, (0xFF, 0xFF, 0xFF))
+
+ ret = ()
+ for f in found:
+ dr_string = ""
+ if f["datarate"] == Crazyradio.DR_2MPS:
+ dr_string = "2M"
+ if f["datarate"] == Crazyradio.DR_250KPS:
+ dr_string = "250K"
+ if f["datarate"] == Crazyradio.DR_1MPS:
+ dr_string = "1M"
+
+ ret += ("radio://0/{}/{}".format(f["channel"], dr_string),)
+
+ return ret
+
+ def scan_interface(self, address):
+ """ Scan interface for Crazyflies """
+ if self.cradio is None:
+ try:
+ self.cradio = Crazyradio()
+ except Exception:
+ print "Crazyradio() didnt initialize"
+ return []
+ else:
+ print "Cannot scann for links while the link is open!"
+ raise Exception("Cannot scann for links while the link is open!")
+
+ # FIXME: implements serial number in the Crazyradio driver!
+ serial = "N/A"
+
+ logger.info("v%s dongle with serial %s found", self.cradio.version,
+ serial)
+ found = []
+
+ if address != None:
+ addr = "{:X}".format(address)
+ new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
+ self.cradio.set_address(new_addr)
+
+ self.cradio.set_arc(1)
+
+ self.cradio.set_data_rate(self.cradio.DR_250KPS)
+
+ if address == None or address == 0xE7E7E7E7E7:
+ found += map(lambda c: ["radio://0/{}/250K".format(c), ""],
+ self._scan_radio_channels())
+ self.cradio.set_data_rate(self.cradio.DR_1MPS)
+ found += map(lambda c: ["radio://0/{}/1M".format(c), ""],
+ self._scan_radio_channels())
+ self.cradio.set_data_rate(self.cradio.DR_2MPS)
+ found += map(lambda c: ["radio://0/{}/2M".format(c), ""],
+ self._scan_radio_channels())
+ else:
+ found += map(lambda c: ["radio://0/{}/250K/{:X}".format(c, address), ""],
+ self._scan_radio_channels())
+ self.cradio.set_data_rate(self.cradio.DR_1MPS)
+ found += map(lambda c: ["radio://0/{}/1M/{:X}".format(c, address), ""],
+ self._scan_radio_channels())
+ self.cradio.set_data_rate(self.cradio.DR_2MPS)
+ found += map(lambda c: ["radio://0/{}/2M/{:X}".format(c, address), ""],
+ self._scan_radio_channels())
+
+ self.cradio.close()
+ self.cradio = None
+
+ return found
+
+ def get_status(self):
+ if self.cradio is None:
+ try:
+ self.cradio = Crazyradio()
+ except USBError as e:
+ return "Cannot open Crazyradio. Permission problem?"\
+ " ({})".format(str(e))
+ except Exception as e:
+ return str(e)
+
+ ver = self.cradio.version
+ self.cradio.close()
+ self.cradio = None
+
+ return "Crazyradio version {}".format(ver)
+
+ def get_name(self):
+ return "radio"
+
+
+# Transmit/receive radio thread
+class _RadioDriverThread (threading.Thread):
+ """
+ Radio link receiver thread used to read data from the
+ Crazyradio USB driver. """
+
+ RETRYCOUNT_BEFORE_DISCONNECT = 10
+
+ def __init__(self, cradio, inQueue, outQueue, link_quality_callback,
+ link_error_callback):
+ """ Create the object """
+ threading.Thread.__init__(self)
+ self.cradio = cradio
+ self.in_queue = inQueue
+ self.out_queue = outQueue
+ self.sp = False
+ self.link_error_callback = link_error_callback
+ self.link_quality_callback = link_quality_callback
+ self.retryBeforeDisconnect = self.RETRYCOUNT_BEFORE_DISCONNECT
+
+ def stop(self):
+ """ Stop the thread """
+ self.sp = True
+ try:
+ self.join()
+ except Exception:
+ pass
+
+ def run(self):
+ """ Run the receiver thread """
+ dataOut = array.array('B', [0xFF])
+ waitTime = 0
+ emptyCtr = 0
+
+ while(True):
+ if (self.sp):
+ break
+
+ try:
+ ackStatus = self.cradio.send_packet(dataOut)
+ except Exception as e:
+ import traceback
+ self.link_error_callback("Error communicating with crazy radio"
+ " ,it has probably been unplugged!\n"
+ "Exception:%s\n\n%s" % (e,
+ traceback.format_exc()))
+
+ # Analise the in data packet ...
+ if ackStatus is None:
+ if (self.link_error_callback is not None):
+ self.link_error_callback("Dongle communication error"
+ " (ackStatus==None)")
+ continue
+
+ if (self.link_quality_callback is not None):
+ self.link_quality_callback((10 - ackStatus.retry) * 10)
+
+ # If no copter, retry
+ if ackStatus.ack is False:
+ self.retryBeforeDisconnect = self.retryBeforeDisconnect - 1
+ if (self.retryBeforeDisconnect == 0 and
+ self.link_error_callback is not None):
+ self.link_error_callback("Too many packets lost")
+ continue
+ self.retryBeforeDisconnect = self.RETRYCOUNT_BEFORE_DISCONNECT
+
+ data = ackStatus.data
+
+ # If there is a copter in range, the packet is analysed and the
+ # next packet to send is prepared
+ if (len(data) > 0):
+ inPacket = CRTPPacket(data[0], list(data[1:]))
+ # print "<- " + inPacket.__str__()
+ self.in_queue.put(inPacket)
+ waitTime = 0
+ emptyCtr = 0
+ else:
+ emptyCtr += 1
+ if (emptyCtr > 10):
+ emptyCtr = 10
+ # Relaxation time if the last 10 packet where empty
+ waitTime = 0.01
+ else:
+ waitTime = 0
+
+ # get the next packet to send of relaxation (wait 10ms)
+ outPacket = None
+ try:
+ outPacket = self.out_queue.get(True, waitTime)
+ except Queue.Empty:
+ outPacket = None
+
+ dataOut = array.array('B')
+
+ if outPacket:
+ # print "-> " + outPacket.__str__()
+ dataOut.append(outPacket.header)
+ for X in outPacket.data:
+ if type(X) == int:
+ dataOut.append(X)
+ else:
+ dataOut.append(ord(X))
+ else:
+ dataOut.append(0xFF)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..5c813e26bef192cc8be323764abc89307a9d2694
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..d6cae55ffd7ed3b37f10f50989a7738ae2be5f56
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+An early serial link driver. This could still be used (after some fixing) to
+run high-speed CRTP with the Crazyflie. The UART can be run at 2Mbit.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['SerialDriver']
+
+from .crtpdriver import CRTPDriver
+
+from .exceptions import WrongUriType
+
+import re
+
+
+class SerialDriver (CRTPDriver):
+ def __init__(self):
+ None
+
+ def connect(self, uri, linkQualityCallback, linkErrorCallback):
+ #check if the URI is a serial URI
+ if not re.search("^serial://", uri):
+ raise WrongUriType("Not a serial URI")
+
+ #Check if it is a valid serial URI
+ uriRe = re.search("^serial://([a-z A-Z 0-9]+)/?([0-9]+)?$", uri)
+ if not uriRe:
+ raise Exception("Invalid serial URI")
+
+ def get_name(self):
+ return "serial"
+
+ def scan_interface(self, address):
+ return []
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cf7f19bdc9799ac60bf5936db90efd5de8f4a75e
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..cb6453b34549256c5887d1bf172c0f06c1cbac00
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.py
@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+""" CRTP UDP Driver. Work either with the UDP server or with an UDP device
+See udpserver.py for the protocol"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['UdpDriver']
+
+
+from .crtpdriver import CRTPDriver
+from .crtpstack import CRTPPacket
+from .exceptions import WrongUriType
+import Queue
+import re
+import struct
+from socket import socket
+
+
+class UdpDriver(CRTPDriver):
+ def __init__(self):
+ None
+
+ def connect(self, uri, linkQualityCallback, linkErrorCallback):
+ #check if the URI is a radio URI
+ if not re.search("^udp://", uri):
+ raise WrongUriType("Not an UDP URI")
+
+ self.queue = Queue.Queue()
+ self.socket = socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self.addr = ("localhost", 7777)
+ self.socket.connect(self.addr)
+
+ #Add this to the server clients list
+ self.socket.sendto("\xFF\x01\x01\x01", self.addr)
+
+ def receive_packet(self, time=0):
+ data, addr = self.socket.recvfrom(1024)
+
+ if data:
+ data = struct.unpack('b' * (len(data) - 1), data[0:len(data) - 1])
+ pk = CRTPPacket()
+ pk.port = data[0]
+ pk.data = data[1:]
+ return pk
+
+ try:
+ if time == 0:
+ return self.rxqueue.get(False)
+ elif time < 0:
+ while True:
+ return self.rxqueue.get(True, 10)
+ else:
+ return self.rxqueue.get(True, time)
+ except Queue.Empty:
+ return None
+
+ def send_packet(self, pk):
+ raw = (pk.port,) + struct.unpack("B"* len(pk.data), pk.data)
+
+ cksum = 0
+ for i in raw:
+ cksum += i
+
+ cksum %= 256
+
+ data = ''.join(chr(v) for v in (raw + (cksum,)))
+
+ #print tuple(data)
+ self.socket.sendto(data, self.addr)
+
+ def close(self):
+ #Remove this from the server clients list
+ self.socket.sendto("\xFF\x01\x02\x02", self.addr)
+
+ def get_name(self):
+ return "udp"
+
+ def scan_interface(self, address):
+ return []
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cb845b82eba07b47050b5b3820bc2441ef2e033a
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.py
new file mode 100755
index 0000000000000000000000000000000000000000..bd4a596ad184ae10d0b3f8f555521013ff7cb400
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.py
@@ -0,0 +1,254 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Crazyflie USB driver.
+
+This driver is used to communicate with the Crazyflie using the USB connection.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['UsbDriver']
+
+import logging
+logger = logging.getLogger(__name__)
+
+from cflib.crtp.crtpdriver import CRTPDriver
+from .crtpstack import CRTPPacket
+from .exceptions import WrongUriType
+import threading
+import Queue
+import re
+import time
+
+from cflib.drivers.cfusb import CfUsb
+from usb import USBError
+
+
+class UsbDriver(CRTPDriver):
+ """ Crazyradio link driver """
+ def __init__(self):
+ """ Create the link driver """
+ CRTPDriver.__init__(self)
+ self.cfusb = None
+ self.uri = ""
+ self.link_error_callback = None
+ self.link_quality_callback = None
+ self.in_queue = None
+ self.out_queue = None
+ self._thread = None
+ self.needs_resending = False
+
+ def connect(self, uri, link_quality_callback, link_error_callback):
+ """
+ Connect the link driver to a specified URI of the format:
+ radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
+
+ The callback for linkQuality can be called at any moment from the
+ driver to report back the link quality in percentage. The
+ callback from linkError will be called when a error occues with
+ an error message.
+ """
+
+ # check if the URI is a radio URI
+ if not re.search("^usb://", uri):
+ raise WrongUriType("Not a radio URI")
+
+ # Open the USB dongle
+ if not re.search("^usb://([0-9]+)$",
+ uri):
+ raise WrongUriType('Wrong radio URI format!')
+
+ uri_data = re.search("^usb://([0-9]+)$",
+ uri)
+
+ self.uri = uri
+
+ if self.cfusb is None:
+ self.cfusb = CfUsb(devid=int(uri_data.group(1)))
+ if self.cfusb.dev:
+ self.cfusb.set_crtp_to_usb(True)
+ time.sleep(1) # Wait for the blocking queues in the firmware to time out
+ else:
+ self.cfusb = None
+ raise Exception("Could not open {}".format(self.uri))
+
+ else:
+ raise Exception("Link already open!")
+
+ # Prepare the inter-thread communication queue
+ self.in_queue = Queue.Queue()
+ # Limited size out queue to avoid "ReadBack" effect
+ self.out_queue = Queue.Queue(50)
+
+ # Launch the comm thread
+ self._thread = _UsbReceiveThread(self.cfusb, self.in_queue,
+ link_quality_callback,
+ link_error_callback)
+ self._thread.start()
+
+ self.link_error_callback = link_error_callback
+
+ def receive_packet(self, time=0):
+ """
+ Receive a packet though the link. This call is blocking but will
+ timeout and return None if a timeout is supplied.
+ """
+ if time == 0:
+ try:
+ return self.in_queue.get(False)
+ except Queue.Empty:
+ return None
+ elif time < 0:
+ try:
+ return self.in_queue.get(True)
+ except Queue.Empty:
+ return None
+ else:
+ try:
+ return self.in_queue.get(True, time)
+ except Queue.Empty:
+ return None
+
+ def send_packet(self, pk):
+ """ Send the packet pk though the link """
+ # if self.out_queue.full():
+ # self.out_queue.get()
+ if (self.cfusb is None):
+ return
+
+ try:
+ dataOut = (pk.header,)
+ dataOut += pk.datat
+ self.cfusb.send_packet(dataOut)
+ except Queue.Full:
+ if self.link_error_callback:
+ self.link_error_callback("UsbDriver: Could not send packet"
+ " to Crazyflie")
+
+ def pause(self):
+ self._thread.stop()
+ self._thread = None
+
+ def restart(self):
+ if self._thread:
+ return
+
+ self._thread = _UsbReceiveThread(self.cfusb, self.in_queue,
+ self.link_quality_callback,
+ self.link_error_callback)
+ self._thread.start()
+
+ def close(self):
+ """ Close the link. """
+ # Stop the comm thread
+ self._thread.stop()
+
+ # Close the USB dongle
+ try:
+ if self.cfusb:
+ self.cfusb.set_crtp_to_usb(False)
+ self.cfusb.close()
+ except Exception as e:
+ # If we pull out the dongle we will not make this call
+ logger.info("Could not close {}".format(e))
+ pass
+ self.cfusb = None
+
+ def scan_interface(self, address):
+ """ Scan interface for Crazyflies """
+ if self.cfusb is None:
+ try:
+ self.cfusb = CfUsb()
+ except Exception as e:
+ logger.warn("Exception while scanning for Crazyflie USB: {}".format(str(e)))
+ return []
+ else:
+ raise Exception("Cannot scan for links while the link is open!")
+
+ # FIXME: implements serial number in the Crazyradio driver!
+ #serial = "N/A"
+
+ found = self.cfusb.scan()
+
+ self.cfusb.close()
+ self.cfusb = None
+
+ return found
+
+ def get_status(self):
+ return "No information available"
+
+ def get_name(self):
+ return "UsbCdc"
+
+
+# Transmit/receive radio thread
+class _UsbReceiveThread (threading.Thread):
+ """
+ Radio link receiver thread used to read data from the
+ Crazyradio USB driver. """
+
+ #RETRYCOUNT_BEFORE_DISCONNECT = 10
+
+ def __init__(self, cfusb, inQueue, link_quality_callback,
+ link_error_callback):
+ """ Create the object """
+ threading.Thread.__init__(self)
+ self.cfusb = cfusb
+ self.in_queue = inQueue
+ self.sp = False
+ self.link_error_callback = link_error_callback
+ self.link_quality_callback = link_quality_callback
+
+ def stop(self):
+ """ Stop the thread """
+ self.sp = True
+ try:
+ self.join()
+ except Exception:
+ pass
+
+ def run(self):
+ """ Run the receiver thread """
+
+ while(True):
+ if (self.sp):
+ break
+ try:
+ # Blocking until USB data available
+ data = self.cfusb.receive_packet()
+ if len(data) > 0:
+ pk = CRTPPacket(data[0], list(data[1:]))
+ self.in_queue.put(pk)
+ except Exception as e:
+ import traceback
+ self.link_error_callback("Error communicating with the Crazyflie"
+ " ,it has probably been unplugged!\n"
+ "Exception:%s\n\n%s" % (e,
+ traceback.format_exc()))
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0706d7d037264bc184b7d997125cf8538a1288fe
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..f074d797a973bcba4f86ae9ed1bcd86112840a72
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Drivers for the link interfaces that can be used by CRTP.
+"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..55c08d1fb913181c63048089e35802f5bfab0ad3
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py
new file mode 100755
index 0000000000000000000000000000000000000000..117fd869e938cb6182d6671801f58bb492d51e72
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py
@@ -0,0 +1,196 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+USB driver for the Crazyflie.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['CfUsb']
+
+
+import os
+import usb
+import logging
+import sys
+import time
+import array
+import binascii
+
+logger = logging.getLogger(__name__)
+
+USB_VID = 0x0483
+USB_PID = 0x5740
+
+try:
+ import usb.core
+ pyusb_backend = None
+ if os.name == "nt":
+ import usb.backend.libusb0 as libusb0
+ pyusb_backend = libusb0.get_backend()
+ pyusb1 = True
+except:
+ pyusb1 = False
+
+
+def _find_devices():
+ """
+ Returns a list of CrazyRadio devices currently connected to the computer
+ """
+ ret = []
+
+ logger.info("Looking for devices....")
+
+ if pyusb1:
+ for d in usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1, backend=pyusb_backend):
+ ret.append(d)
+ else:
+ busses = usb.busses()
+ for bus in busses:
+ for device in bus.devices:
+ if device.idVendor == USB_VID:
+ if device.idProduct == USB_PID:
+ ret += [device, ]
+
+ return ret
+
+
+class CfUsb:
+ """ Used for communication with the Crazyradio USB dongle """
+
+ def __init__(self, device=None, devid=0):
+ """ Create object and scan for USB dongle if no device is supplied """
+ self.dev = None
+ self.handle = None
+ self._last_write = 0
+ self._last_read = 0
+
+ if device is None:
+ devices = _find_devices()
+ try:
+ self.dev = devices[devid]
+ except Exception:
+ self.dev = None
+
+
+ if self.dev:
+ if (pyusb1 is True):
+ self.dev.set_configuration(1)
+ self.handle = self.dev
+ self.version = float("{0:x}.{1:x}".format(self.dev.bcdDevice >> 8,
+ self.dev.bcdDevice & 0x0FF))
+ else:
+ self.handle = self.dev.open()
+ self.handle.setConfiguration(1)
+ self.handle.claimInterface(0)
+ self.version = float(self.dev.deviceVersion)
+
+ def get_serial(self):
+ return usb.util.get_string(self.dev, 255, self.dev.iSerialNumber)
+
+ def close(self):
+ if (pyusb1 is False):
+ if self.handle:
+ self.handle.releaseInterface()
+ self.handle.reset()
+ else:
+ if self.dev:
+ self.dev.reset()
+
+ self.handle = None
+ self.dev = None
+
+ def scan(self):
+ # TODO: Currently only supports one device
+ if self.dev:
+ return [("usb://0","")]
+ return []
+
+ def set_crtp_to_usb(self, crtp_to_usb):
+ if crtp_to_usb:
+ _send_vendor_setup(self.handle, 0x01, 0x01, 1, ())
+ else:
+ _send_vendor_setup(self.handle, 0x01, 0x01, 0, ())
+
+ ### Data transfers ###
+ def send_packet(self, dataOut):
+ """ Send a packet and receive the ack from the radio dongle
+ The ack contains information about the packet transmition
+ and a data payload if the ack packet contained any """
+ try:
+ if (pyusb1 is False):
+ count = self.handle.bulkWrite(1, dataOut, 20)
+ else:
+ count = self.handle.write(endpoint=1, data=dataOut, timeout=20)
+ except usb.USBError as e:
+ pass
+
+
+ def receive_packet(self):
+ dataIn = ()
+ try:
+ if (pyusb1 is False):
+ dataIn = self.handle.bulkRead(0x81, 64, 20)
+ else:
+ dataIn = self.handle.read(0x81, 64, timeout=20)
+ except usb.USBError as e:
+ try:
+ if e.backend_error_code == -7 or e.backend_error_code == -116:
+ # Normal, the read was empty
+ pass
+ else:
+ raise IOError("Crazyflie disconnected")
+ except AttributeError as e:
+ # pyusb < 1.0 doesn't implement getting the underlying error
+ # number and it seems as if it's not possible to detect
+ # if the cable is disconnected. So this detection is not
+ # supported, but the "normal" case will work.
+ pass
+
+ return dataIn
+
+
+
+#Private utility functions
+def _send_vendor_setup(handle, request, value, index, data):
+ if pyusb1:
+ handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value,
+ wIndex=index, timeout=1000, data_or_wLength=data)
+ else:
+ handle.controlMsg(usb.TYPE_VENDOR, request, data, value=value,
+ index=index, timeout=1000)
+
+
+def _get_vendor_setup(handle, request, value, index, length):
+ if pyusb1:
+ return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request,
+ wValue=value, wIndex=index, timeout=1000,
+ data_or_wLength=length)
+ else:
+ return handle.controlMsg(usb.TYPE_VENDOR | 0x80, request, length,
+ value=value, index=index, timeout=1000)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..a9e4663144f91afb1d934d409fa194847e80b9b9
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py
new file mode 100755
index 0000000000000000000000000000000000000000..8bdcfebe6cfc5fbba325a8d0cb2fb057215eec94
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py
@@ -0,0 +1,295 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+USB driver for the Crazyradio USB dongle.
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Crazyradio']
+
+
+import os
+import usb
+import logging
+logger = logging.getLogger(__name__)
+
+#USB parameters
+CRADIO_VID = 0x1915
+CRADIO_PID = 0x7777
+
+# Dongle configuration requests
+#See http://wiki.bitcraze.se/projects:crazyradio:protocol for documentation
+SET_RADIO_CHANNEL = 0x01
+SET_RADIO_ADDRESS = 0x02
+SET_DATA_RATE = 0x03
+SET_RADIO_POWER = 0x04
+SET_RADIO_ARD = 0x05
+SET_RADIO_ARC = 0x06
+ACK_ENABLE = 0x10
+SET_CONT_CARRIER = 0x20
+SCANN_CHANNELS = 0x21
+LAUNCH_BOOTLOADER = 0xFF
+
+try:
+ import usb.core
+ pyusb_backend = None
+ if os.name == "nt":
+ import usb.backend.libusb0 as libusb0
+ pyusb_backend = libusb0.get_backend()
+ pyusb1 = True
+except:
+ pyusb1 = False
+
+
+def _find_devices():
+ """
+ Returns a list of CrazyRadio devices currently connected to the computer
+ """
+ ret = []
+
+ if pyusb1:
+ for d in usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1, backend=pyusb_backend):
+ ret.append(d)
+ else:
+ busses = usb.busses()
+ for bus in busses:
+ for device in bus.devices:
+ if device.idVendor == CRADIO_VID:
+ if device.idProduct == CRADIO_PID:
+ ret += [device, ]
+ return ret
+
+
+class _radio_ack:
+ ack = False
+ powerDet = False
+ retry = 0
+ data = ()
+
+
+class Crazyradio:
+ """ Used for communication with the Crazyradio USB dongle """
+ #configuration constants
+ DR_250KPS = 0
+ DR_1MPS = 1
+ DR_2MPS = 2
+
+ P_M18DBM = 0
+ P_M12DBM = 1
+ P_M6DBM = 2
+ P_0DBM = 3
+
+ def __init__(self, device=None, devid=0):
+ """ Create object and scan for USB dongle if no device is supplied """
+ if device is None:
+ try:
+ device = _find_devices()[devid]
+ except Exception:
+ print "Cannot find a Crazyradio Dongle"
+ raise Exception("Cannot find a Crazyradio Dongle")
+
+ self.dev = device
+
+ if (pyusb1 is True):
+ self.dev.set_configuration(1)
+ self.handle = self.dev
+ self.version = float("{0:x}.{1:x}".format(self.dev.bcdDevice >> 8,
+ self.dev.bcdDevice & 0x0FF))
+ else:
+ self.handle = self.dev.open()
+ self.handle.setConfiguration(1)
+ self.handle.claimInterface(0)
+ self.version = float(self.dev.deviceVersion)
+
+ if self.version < 0.3:
+ raise "This driver requires Crazyradio firmware V0.3+"
+ print "This driver requires Crazyradio firmware V0.3+"
+
+ if self.version < 0.4:
+ logger.warning("You should update to Crazyradio firmware V0.4+")
+ print "You should update to Crazyradio firmware V0.4+"
+
+
+ #Reset the dongle to power up settings
+ self.set_data_rate(self.DR_2MPS)
+ self.set_channel(2)
+ self.arc = -1
+ if self.version >= 0.4:
+ self.set_cont_carrier(False)
+ self.set_address((0xE7,) * 5)
+ self.set_power(self.P_0DBM)
+ self.set_arc(3)
+ self.set_ard_bytes(32)
+ print "crazyradio initialized"
+
+ def close(self):
+ if (pyusb1 is False):
+ if self.handle:
+ self.handle.releaseInterface()
+ self.handle.reset()
+ else:
+ if self.dev:
+ self.dev.reset()
+
+ self.handle = None
+ self.dev = None
+
+ ### Dongle configuration ###
+ def set_channel(self, channel):
+ """ Set the radio channel to be used """
+ _send_vendor_setup(self.handle, SET_RADIO_CHANNEL, channel, 0, ())
+
+ def set_address(self, address):
+ """ Set the radio address to be used"""
+ if len(address) != 5:
+ print "Crazyradio: the radio address shall be 5 bytes long"
+ raise Exception("Crazyradio: the radio address shall be 5"
+ " bytes long")
+
+ _send_vendor_setup(self.handle, SET_RADIO_ADDRESS, 0, 0, address)
+
+ def set_data_rate(self, datarate):
+ """ Set the radio datarate to be used """
+ _send_vendor_setup(self.handle, SET_DATA_RATE, datarate, 0, ())
+
+ def set_power(self, power):
+ """ Set the radio power to be used """
+ _send_vendor_setup(self.handle, SET_RADIO_POWER, power, 0, ())
+
+ def set_arc(self, arc):
+ """ Set the ACK retry count for radio communication """
+ _send_vendor_setup(self.handle, SET_RADIO_ARC, arc, 0, ())
+ self.arc = arc
+
+ def set_ard_time(self, us):
+ """ Set the ACK retry delay for radio communication """
+ # Auto Retransmit Delay:
+ # 0000 - Wait 250uS
+ # 0001 - Wait 500uS
+ # 0010 - Wait 750uS
+ # ........
+ # 1111 - Wait 4000uS
+
+ # Round down, to value representing a multiple of 250uS
+ t = int((us / 250) - 1)
+ if (t < 0):
+ t = 0
+ if (t > 0xF):
+ t = 0xF
+ _send_vendor_setup(self.handle, SET_RADIO_ARD, t, 0, ())
+
+ def set_ard_bytes(self, nbytes):
+ _send_vendor_setup(self.handle, SET_RADIO_ARD, 0x80 | nbytes, 0, ())
+
+ def set_cont_carrier(self, active):
+ if active:
+ _send_vendor_setup(self.handle, SET_CONT_CARRIER, 1, 0, ())
+ else:
+ _send_vendor_setup(self.handle, SET_CONT_CARRIER, 0, 0, ())
+
+ def _has_fw_scan(self):
+ #return self.version >= 0.5
+ # FIXME: Mitigation for Crazyradio firmware bug #9
+ return False
+
+ def scan_selected(self, selected, packet):
+ result = ()
+ for s in selected:
+ self.set_channel(s["channel"])
+ self.set_data_rate(s["datarate"])
+ status = self.send_packet(packet)
+ if status and status.ack:
+ result = result + (s,)
+
+ return result
+
+
+ def scan_channels(self, start, stop, packet):
+ if self._has_fw_scan(): # Fast firmware-driven scan
+ _send_vendor_setup(self.handle, SCANN_CHANNELS, start, stop,
+ packet)
+ return tuple(_get_vendor_setup(self.handle, SCANN_CHANNELS,
+ 0, 0, 64))
+ else: # Slow PC-driven scan
+ result = tuple()
+ for i in range(start, stop + 1):
+ self.set_channel(i)
+ status = self.send_packet(packet)
+ if status and status.ack:
+ result = result + (i,)
+ return result
+
+ ### Data transferts ###
+ def send_packet(self, dataOut):
+ """ Send a packet and receive the ack from the radio dongle
+ The ack contains information about the packet transmition
+ and a data payload if the ack packet contained any """
+ ackIn = None
+ data = None
+ try:
+ if (pyusb1 is False):
+ self.handle.bulkWrite(1, dataOut, 1000)
+ data = self.handle.bulkRead(0x81, 64, 1000)
+ else:
+ self.handle.write(endpoint=1, data=dataOut, timeout=1000)
+ data = self.handle.read(0x81, 64, timeout=1000)
+ except usb.USBError:
+ pass
+
+ if data is not None:
+ ackIn = _radio_ack()
+ if data[0] != 0:
+ ackIn.ack = (data[0] & 0x01) != 0
+ ackIn.powerDet = (data[0] & 0x02) != 0
+ ackIn.retry = data[0] >> 4
+ ackIn.data = data[1:]
+ else:
+ ackIn.retry = self.arc
+
+ return ackIn
+
+
+#Private utility functions
+def _send_vendor_setup(handle, request, value, index, data):
+ if pyusb1:
+ handle.ctrl_transfer(usb.TYPE_VENDOR, request, wValue=value,
+ wIndex=index, timeout=1000, data_or_wLength=data)
+ else:
+ handle.controlMsg(usb.TYPE_VENDOR, request, data, value=value,
+ index=index, timeout=1000)
+
+
+def _get_vendor_setup(handle, request, value, index, length):
+ if pyusb1:
+ return handle.ctrl_transfer(usb.TYPE_VENDOR | 0x80, request,
+ wValue=value, wIndex=index, timeout=1000,
+ data_or_wLength=length)
+ else:
+ return handle.controlMsg(usb.TYPE_VENDOR | 0x80, request, length,
+ value=value, index=index, timeout=1000)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d1609aa6f8b03c5cca508565b9065d939f830156
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..dd277b671ca49039c8196b9338e923d4dcd3b06e
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Various utilities that is needed by the cflib.
+"""
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9cb912d8126dc08dbd5fbdcd9e63c645575bff6f
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py
new file mode 100755
index 0000000000000000000000000000000000000000..7a90f058a3e9d7fd1a88208642a455c5eeb860c5
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py
@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Callback objects used in the Crazyflie library
+"""
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Caller']
+
+
+class Caller():
+ """ An object were callbacks can be registered and called """
+
+ def __init__(self):
+ """ Create the object """
+ self.callbacks = []
+
+ def add_callback(self, cb):
+ """ Register cb as a new callback. Will not register duplicates. """
+ if ((cb in self.callbacks) is False):
+ self.callbacks.append(cb)
+
+ def remove_callback(self, cb):
+ """ Un-register cb from the callbacks """
+ self.callbacks.remove(cb)
+
+ def call(self, *args):
+ """ Call the callbacks registered with the arguments args """
+ for cb in self.callbacks:
+ cb(*args)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7a74eeba740be7823fb8adb7248b5c4ead6e1897
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc differ
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cfzmq.py b/pps_ws/src/d_fall_pps/crazyradio/cfzmq.py
new file mode 100755
index 0000000000000000000000000000000000000000..66d2b8c1276c6ad1c91a247173de3b533db79d58
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/cfzmq.py
@@ -0,0 +1,366 @@
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2015 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+# MA 02110-1301, USA.
+
+"""
+Server used to connect to a Crazyflie using ZMQ.
+"""
+
+import sys
+import os
+import logging
+import signal
+import zmq
+import Queue
+from threading import Thread
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+if os.name == 'posix':
+ print 'Disabling standard output for libraries!'
+ stdout = os.dup(1)
+ os.dup2(os.open('/dev/null', os.O_WRONLY), 1)
+ sys.stdout = os.fdopen(stdout, 'w')
+
+# set SDL to use the dummy NULL video driver,
+# so it doesn't need a windowing system.
+os.environ["SDL_VIDEODRIVER"] = "dummy"
+
+# Main command socket for control (ping/pong)
+ZMQ_SRV_PORT = 2000
+# Log data socket (publish)
+ZMQ_LOG_PORT = 2001
+# Param value updated (publish)
+ZMQ_PARAM_PORT = 2002
+# Async event for connection, like connection lost (publish)
+ZMQ_CONN_PORT = 2003
+# Control set-poins for Crazyflie (pull)
+ZMQ_CTRL_PORT = 2004
+
+# Timeout before giving up when verifying param write
+PARAM_TIMEOUT = 2
+# Timeout before giving up connection
+CONNECT_TIMEOUT = 5
+# Timeout before giving up adding/starting log config
+LOG_TIMEOUT = 10
+
+logger = logging.getLogger(__name__)
+
+class _SrvThread(Thread):
+
+ def __init__(self, socket, log_socket, param_socket, conn_socket, cf, *args):
+ super(_SrvThread, self).__init__(*args)
+ self._socket = socket
+ self._log_socket = log_socket
+ self._param_socket = param_socket
+ self._conn_socket = conn_socket
+ self._cf = cf
+
+ self._cf.connected.add_callback(self._connected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_requested.add_callback(self._connection_requested)
+ self._cf.param.all_updated.add_callback(self._tocs_updated)
+ self._cf.param.all_update_callback.add_callback(self._all_param_update)
+
+ self._conn_queue = Queue.Queue(1)
+ self._param_queue = Queue.Queue(1)
+ self._log_started_queue = Queue.Queue(1)
+ self._log_added_queue = Queue.Queue(1)
+
+ self._logging_configs = {}
+
+ def _connection_requested(self, uri):
+ conn_ev = {"version": 1, "event": "requested", "uri": uri}
+ self._conn_socket.send_json(conn_ev)
+
+ def _connected(self, uri):
+ conn_ev = {"version": 1, "event": "connected", "uri": uri}
+ self._conn_socket.send_json(conn_ev)
+
+ def _connection_failed(self, uri, msg):
+ logger.info("Connection failed to {}: {}".format(uri, msg))
+ resp = {"version": 1, "status": 1, "msg": msg}
+ self._conn_queue.put_nowait(resp)
+ conn_ev = {"version": 1, "event": "failed", "uri": uri, "msg": msg}
+ self._conn_socket.send_json(conn_ev)
+
+ def _connection_lost(self, uri, msg):
+ conn_ev = {"version": 1, "event": "lost", "uri": uri, "msg": msg}
+ self._conn_socket.send_json(conn_ev)
+
+ def _disconnected(self, uri):
+ conn_ev = {"version": 1, "event": "disconnected", "uri": uri}
+ self._conn_socket.send_json(conn_ev)
+
+ def _tocs_updated(self):
+ # First do the log
+ log_toc = self._cf.log.toc.toc
+ log = {}
+ for group in log_toc:
+ log[group] = {}
+ for name in log_toc[group]:
+ log[group][name] = {"type": log_toc[group][name].ctype}
+ # The the params
+ param_toc = self._cf.param.toc.toc
+ param = {}
+ for group in param_toc:
+ param[group] = {}
+ for name in param_toc[group]:
+ param[group][name] = {
+ "type": param_toc[group][name].ctype,
+ "access": "RW" if param_toc[group][name].access == 0 else "RO",
+ "value": self._cf.param.values[group][name]}
+
+ resp = {"version": 1, "status": 0, "log": log, "param": param}
+ self._conn_queue.put_nowait(resp)
+
+ def _handle_scanning(self):
+ resp = {"version": 1}
+ interfaces = cflib.crtp.scan_interfaces()
+ resp["interfaces"] = []
+ for i in interfaces:
+ resp["interfaces"].append({"uri": i[0], "info": i[1]})
+ return resp
+
+ def _handle_connect(self, uri):
+ self._cf.open_link(uri)
+ return self._conn_queue.get(block=True)
+
+ def _logging_started(self, conf, started):
+ out = {"version": 1, "name": conf.name}
+ if started:
+ out["event"] = "started"
+ else:
+ out["event"] = "stopped"
+ self._log_socket.send_json(out)
+ self._log_started_queue.put_nowait(started)
+
+ def _logging_added(self, conf, added):
+ out = {"version": 1, "name": conf.name}
+ if added:
+ out["event"] = "created"
+ else:
+ out["event"] = "deleted"
+ self._log_socket.send_json(out)
+ self._log_added_queue.put_nowait(added)
+
+ def _handle_logging(self, data):
+ resp = {"version": 1}
+ if data["action"] == "create":
+ lg = LogConfig(data["name"], data["period"])
+ for v in data["variables"]:
+ lg.add_variable(v)
+ lg.started_cb.add_callback(self._logging_started)
+ lg.added_cb.add_callback(self._logging_added)
+ try:
+ lg.data_received_cb.add_callback(self._logdata_callback)
+ self._logging_configs[data["name"]] = lg
+ self._cf.log.add_config(lg)
+ lg.create()
+ self._log_added_queue.get(block=True, timeout=LOG_TIMEOUT)
+ resp["status"] = 0
+ except KeyError as e:
+ resp["status"] = 1
+ resp["msg"] = str(e)
+ except AttributeError as e:
+ resp["status"] = 2
+ resp["msg"] = str(e)
+ except Queue.Empty:
+ resp["status"] = 3
+ resp["msg"] = "Log configuration did not start"
+ if data["action"] == "start":
+ try:
+ self._logging_configs[data["name"]].start()
+ self._log_started_queue.get(block=True, timeout=LOG_TIMEOUT)
+ resp["status"] = 0
+ except KeyError as e:
+ resp["status"] = 1
+ resp["msg"] = "{} config not found".format(str(e))
+ except Queue.Empty:
+ resp["status"] = 2
+ resp["msg"] = "Log configuration did not stop"
+ if data["action"] == "stop":
+ try:
+ self._logging_configs[data["name"]].stop()
+ self._log_started_queue.get(block=True, timeout=LOG_TIMEOUT)
+ resp["status"] = 0
+ except KeyError as e:
+ resp["status"] = 1
+ resp["msg"] = "{} config not found".format(str(e))
+ except Queue.Empty:
+ resp["status"] = 2
+ resp["msg"] = "Log configuration did not stop"
+ if data["action"] == "delete":
+ try:
+ self._logging_configs[data["name"]].delete()
+ self._log_added_queue.get(block=True, timeout=LOG_TIMEOUT)
+ resp["status"] = 0
+ except KeyError as e:
+ resp["status"] = 1
+ resp["msg"] = "{} config not found".format(str(e))
+ except Queue.Empty:
+ resp["status"] = 2
+ resp["msg"] = "Log configuration did not stop"
+
+ return resp
+
+ def _handle_param(self, data):
+ resp = {"version": 1}
+ group = data["name"].split(".")[0]
+ name = data["name"].split(".")[1]
+ self._cf.param.add_update_callback(group=group, name=name,
+ cb=self._param_callback)
+ try:
+ self._cf.param.set_value(data["name"], str(data["value"]))
+ answer = self._param_queue.get(block=True, timeout=PARAM_TIMEOUT)
+ resp["name"] = answer["name"]
+ resp["value"] = answer["value"]
+ resp["status"] = 0
+ except KeyError as e:
+ resp["status"] = 1
+ resp["msg"] = str(e)
+ except AttributeError as e:
+ resp["status"] = 2
+ resp["msg"] = str(e)
+ except Queue.Empty:
+ resp["status"] = 3
+ resp["msg"] = "Timeout when setting parameter" \
+ "{}".format(data["name"])
+ return resp
+
+ def _all_param_update(self, name, value):
+ resp = {"version": 1, "name": name, "value": value}
+ self._param_socket.send_json(resp)
+
+ def _param_callback(self, name, value):
+ group = name.split(".")[0]
+ name_short = name.split(".")[1]
+ self._cf.param.remove_update_callback(group=group, name=name_short)
+ self._param_queue.put_nowait({"name": name, "value": value})
+
+ def _logdata_callback(self, ts, data, conf):
+ out = {"version": 1, "name": conf.name, "event": "data",
+ "timestamp": ts, "variables": {}}
+ for d in data:
+ out["variables"][d] = data[d]
+ self._log_socket.send_json(out)
+
+ def run(self):
+ logger.info("Starting server thread")
+ while True:
+ # Wait for the command
+ cmd = self._socket.recv_json()
+ response = {"version": 1}
+ logger.info("Got command {}".format(cmd))
+ if cmd["cmd"] == "scan":
+ response = self._handle_scanning()
+ elif cmd["cmd"] == "connect":
+ response = self._handle_connect(cmd["uri"])
+ elif cmd["cmd"] == "disconnect":
+ self._cf.close_link()
+ response["status"] = 0
+ elif cmd["cmd"] == "log":
+ response = self._handle_logging(cmd)
+ elif cmd["cmd"] == "param":
+ response = self._handle_param(cmd)
+ else:
+ response["status"] = 0xFF
+ response["msg"] = "Unknown command {}".format(cmd["cmd"])
+ self._socket.send_json(response)
+
+
+class _CtrlThread(Thread):
+ def __init__(self, socket, cf, *args):
+ super(_CtrlThread, self).__init__(*args)
+ self._socket = socket
+ self._cf = cf
+
+ def run(self):
+ while True:
+ cmd = self._socket.recv_json()
+ self._cf.commander.send_setpoint(cmd["roll"], cmd["pitch"],
+ cmd["yaw"], cmd["thrust"])
+
+
+class ZMQServer():
+ """Crazyflie ZMQ server"""
+
+ def __init__(self, base_url):
+ """Start threads and bind ports"""
+ cflib.crtp.init_drivers(enable_debug_driver=True)
+ self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
+ rw_cache=sys.path[1]+"/cache")
+
+ signal.signal(signal.SIGINT, signal.SIG_DFL)
+
+ self._base_url = base_url
+ self._context = zmq.Context()
+
+ cmd_srv = self._bind_zmq_socket(zmq.REP, "cmd", ZMQ_SRV_PORT)
+ log_srv = self._bind_zmq_socket(zmq.PUB, "log", ZMQ_LOG_PORT)
+ param_srv = self._bind_zmq_socket(zmq.PUB, "param", ZMQ_PARAM_PORT)
+ ctrl_srv = self._bind_zmq_socket(zmq.PULL, "ctrl", ZMQ_CTRL_PORT)
+ conn_srv = self._bind_zmq_socket(zmq.PUB, "conn", ZMQ_CONN_PORT)
+
+ self._scan_thread = _SrvThread(cmd_srv, log_srv, param_srv, conn_srv,
+ self._cf)
+ self._scan_thread.start()
+
+ self._ctrl_thread = _CtrlThread(ctrl_srv, self._cf)
+ self._ctrl_thread.start()
+
+ def _bind_zmq_socket(self, pattern, name, port):
+ srv = self._context.socket(pattern)
+ srv_addr = "{}:{}".format(self._base_url, port)
+ srv.bind(srv_addr)
+ logger.info("Biding ZMQ {} server"
+ "at {}".format(name, srv_addr))
+ return srv
+
+
+def main():
+ """Main Crazyflie ZMQ application"""
+ import argparse
+
+ parser = argparse.ArgumentParser(prog="cfzmq")
+ parser.add_argument("-u", "--url", action="store", dest="url", type=str,
+ default="tcp://127.0.0.1",
+ help="URL where ZMQ will accept connections")
+ parser.add_argument("-d", "--debug", action="store_true", dest="debug",
+ help="Enable debug output")
+ (args, unused) = parser.parse_known_args()
+
+ if args.debug:
+ logging.basicConfig(level=logging.DEBUG)
+ else:
+ logging.basicConfig(level=logging.INFO)
+
+ ZMQServer(args.url)
+
+ # CRTL-C to exit
diff --git a/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.py b/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.py
new file mode 100755
index 0000000000000000000000000000000000000000..1d24fd789967142deb9afdc452642142f76692ee
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.py
@@ -0,0 +1,38 @@
+# -*- coding: utf-8 -*-
+#
+# || ____ _ __
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc., 51
+# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+"""
+In order to use the Leap Motion as an input device reader the libraries from
+the Leap Motion SDK has to be placed in this directory. This can be done by
+downloading the SDK from the http://developer.leapmotion.com website, unpacking
+it and copying the following files:
+
+ * LeapSDK\lib\Leap.py -> leapsdk\Leap.py
+ * LeapSDK\lib\<your_arch>\LeapPython.so -> leapsdk\LeapPython.so
+ * LeapSDK\lib\<your_arch>\libLeap.so -> leapsdk\libLeap.so
+"""
+
+
diff --git a/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.pyc b/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.pyc
new file mode 100755
index 0000000000000000000000000000000000000000..e6513482535ac9c98b047a49e71c2c2a61f75aa9
Binary files /dev/null and b/pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.pyc differ
diff --git a/pps_ws/src/d_fall_pps/msg/ControllerOutput.msg b/pps_ws/src/d_fall_pps/msg/ControlParameters.msg
similarity index 55%
rename from pps_ws/src/d_fall_pps/msg/ControllerOutput.msg
rename to pps_ws/src/d_fall_pps/msg/ControlParameters.msg
index 455f85572d9cf26908e22070e3cb895d4b40e58c..bcaa8f384601c84dacad3a67996b33581d2a6fb0 100644
--- a/pps_ws/src/d_fall_pps/msg/ControllerOutput.msg
+++ b/pps_ws/src/d_fall_pps/msg/ControlParameters.msg
@@ -1,7 +1,4 @@
-this is copied from dusan and might therefore need modification
-also you need to add this file in the CMakeLists.txt
-
-
+#copied from Dusan, eventually remove onboardControllerType
float32 roll
float32 pitch
float32 yaw
diff --git a/pps_ws/src/d_fall_pps/src/ViconDataNode.cpp b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
similarity index 81%
rename from pps_ws/src/d_fall_pps/src/ViconDataNode.cpp
rename to pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
index 61ba7e0271531bbc57ed7f604cdb1102f1bd7818..3ef987b3d2fae818c156950cf5f9c63fb7b38203 100644
--- a/pps_ws/src/d_fall_pps/src/ViconDataNode.cpp
+++ b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
@@ -1,13 +1,18 @@
-///////////////////////////////////////////////////////////////////////////////
+// ROS node that publishes the data from the Vicon system
+// Copyright (C) 2017 Dusan Zikovic, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
-// Copyright (C) OMG Plc 2009.
-// All rights reserved. This software is protected by copyright
-// law and international treaties. No part of this software / document
-// may be reproduced or distributed in any form or by any means,
-// whether transiently or incidentally to some other use of this software,
-// without the written permission of the copyright owner.
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
//
-///////////////////////////////////////////////////////////////////////////////
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <string.h>
#include "DataStreamClient.h"
@@ -17,7 +22,7 @@
using namespace ViconDataStreamSDK::CPP;
int main(int argc, char* argv[]) {
- ros::init(argc, argv, "ViconDataNode");
+ ros::init(argc, argv, "ViconDataPublisher");
ros::NodeHandle nodeHandle("~");
ros::Time::init();