diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 47c918c6d926a15bfec44ab581e0282aafa5f604..29437afb06a23b6e7559715db2008aa61c165994 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -11,6 +11,10 @@ import time, sys
import struct
import logging
+import rosbag
+from std_msgs.msg import Float32
+from std_msgs.msg import String
+
# Add library
#sys.path.append("lib")
@@ -22,7 +26,7 @@ from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
import cflib.drivers.crazyradio
-# Logging import
+# Logging import(*
from cflib.crazyflie.log import LogConfig
# Logging settings
@@ -33,6 +37,8 @@ CONTROLLER_ANGLE = 1
CONTROLLER_RATE = 0
RAD_TO_DEG = 57.296
+bag = rosbag.Bag('/home/d-fall/D-FaLL-System/pps_ws/src/d_fall_pps/test.bag', 'w')
+
class PPSRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
@@ -62,7 +68,21 @@ class PPSRadioClient:
# Connect to the Crazyflie
print "Connecting to %s" % link_uri
self._cf.open_link(link_uri)
-
+
+
+ def _data_received_callback(self, timestamp, data, logconf):
+ print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
+ batteryVolt = Float32()
+ batteryVolt.data = data["pm.vbat"]
+ str = String()
+ str.data = 'foo'
+ bag.write('onboard', str)
+ bag.write('onboard', batteryVolt)
+ print "status check"
+
+
+ def _logging_error(self, logconf, msg):
+ print "Error when logging %s" % logconf.name
def _connected(self, link_uri):
"""
@@ -70,6 +90,24 @@ class PPSRadioClient:
quadrotor is established.
"""
rospy.loginfo("Connection to %s successful: " % link_uri)
+
+
+ # Config for Logging
+ logconf = LogConfig("LoggingTest", 1000)
+ logconf.add_variable("stabilizer.roll", "float");
+ logconf.add_variable("stabilizer.pitch", "float");
+ logconf.add_variable("stabilizer.yaw", "float");
+ logconf.add_variable("pm.vbat", "float");
+
+ self._cf.log.add_config(logconf)
+ if logconf.valid:
+ logconf.data_received_cb.add_callback(self._data_received_callback)
+ logconf.error_cb.add_callback(self._logging_error)
+ logconf.start()
+ print "logconf valid"
+ else:
+ print "logconf invalid"
+
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
@@ -84,6 +122,9 @@ class PPSRadioClient:
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
rospy.logwarn("Disconnected from %s" % link_uri)
+ bag.close()
+ rospy.loginfo("bag closed")
+
def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
pk = CRTPPacket()
@@ -98,6 +139,8 @@ def controlCommandCallback(data):
#cmd1..4 must not be 0, as crazyflie onboard controller resets!
#pitch and yaw are inverted on crazyflie controller
cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
+
+
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index cfcc3529f984ae43fe41cd9184843d9780878cc3..91fbaf884620b4c6629cb77a85a7922158da862b 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,6 +1,6 @@
teamName: "Two"
-crazyFlieName: "cfFour"
-crazyFlieAddress: "radio://0/99/2M"
+crazyFlieName: "cfTwo"
+crazyFlieAddress: "radio://0/69/2M"
safeController: "SafeControllerService/RateController"
customController: ""
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index a927e7be082f589742e74dad5886ed3f2525382b..595348ed07ce7e7ad5bb69efaed5c7e95efecfee 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -15,5 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
-defaultSetpoint: [-0.7, -0.1, 0.25, 0.0]
-
+defaultSetpoint: [-0.5, 0.0, 0.4, 1.2]
diff --git a/pps_ws/src/d_fall_pps/param/StudentID b/pps_ws/src/d_fall_pps/param/StudentID
index 00750edc07d6415dcc07ae0351e9397b0222b7ba..7ed6ff82de6bcc2a78243fc9c54d3ef5ac14da69 100644
--- a/pps_ws/src/d_fall_pps/param/StudentID
+++ b/pps_ws/src/d_fall_pps/param/StudentID
@@ -1 +1 @@
-3
+5
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 725f81a7682cf621fd649fbbae9934b38bc5d3e0..9d67cad780f00fe662cbe87ec5cab7af8b6256f2 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -89,7 +89,7 @@ void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData data = *it;
- ROS_INFO_STREAM(data);
+ //ROS_INFO_STREAM(data);
if(data.crazyflieName == crazyflieName) {
Controller controllerCall;
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 7aa2d9168faf7834e271ef8ecd058fecaaffae03..532cafb63d3a8a33a534c6d774e59d4143cdf7d9 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -238,7 +238,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//HINWEIS: übersteuern beim outYaw wenn man 180 Grad zum yaw-Setpoint startet
//nach Multiplikation mit 0.5 gibt es den Effekt nicht mehr -> mit Paul besprechen....
- outYaw *= 0.5;
+ //outYaw *= 0.5;
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;