diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index dd2b8629797f5b784ae4e9a5e9e6ede9b75850ce..62da8744dd3c95813cd3e5c31f61225f2a39fba5 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -34,9 +34,15 @@ from cflib.crazyflie.log import LogConfig
# Logging settings
logging.basicConfig(level=logging.ERROR)
-CONTROLLER_MOTOR = 2
-CONTROLLER_ANGLE = 1
-CONTROLLER_RATE = 0
+# CONTROLLER_MOTOR = 2
+# CONTROLLER_ANGLE = 1
+# CONTROLLER_RATE = 0
+
+
+TYPE_PPS_MOTORS = 6
+TYPE_PPS_RATE = 7
+TYPE_PPS_ANGLE = 8
+
RAD_TO_DEG = 57.296
# CrazyRadio states:
@@ -216,13 +222,30 @@ class PPSRadioClient:
self.logconf.delete()
rospy.loginfo("logconf deleted")
+ def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
+ pk = CRTPPacket()
+ pk.port = CRTPPort.COMMANDER_GENERIC
+ pk.data = struct.pack('<BHHHH', TYPE_PPS_MOTORS, cmd1, cmd2, cmd3, cmd4)
+ self._cf.send_packet(pk)
+
+ def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
+ pk = CRTPPacket()
+ pk.port = CRTPPort.COMMANDER_GENERIC
+ pk.data = struct.pack('<BHHHHfff', TYPE_PPS_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
+ self._cf.send_packet(pk)
- def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
+ def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
pk = CRTPPacket()
- pk.port = CRTPPort.COMMANDER
- pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
+ pk.port = CRTPPort.COMMANDER_GENERIC
+ pk.data = struct.pack('<BHHHHfff', TYPE_PPS_ANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
self._cf.send_packet(pk)
+ # 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 * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
+ # self._cf.send_packet(pk)
+
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
print "crazyRadio command received %s" % msg.data
@@ -249,8 +272,15 @@ 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 data.onboardControllerType == TYPE_PPS_MOTORS:
+ cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
+
+ elif data.onboardControllerType == TYPE_PPS_RATE:
+ cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
+ elif data.onboardControllerType == TYPE_PPS_ANGLE:
+ cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
+ # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/TestCF.py b/pps_ws/src/d_fall_pps/crazyradio/TestCF.py
index 2682260ef4dbaf1dde6d0eb5dd081e8b79586c60..c5fa4f44fad954a0c86f728de59046aade762bd3 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/TestCF.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/TestCF.py
@@ -204,7 +204,7 @@ class PPSRadioClient:
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', TYPE_PPSRATE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
+ pk.data = struct.pack('<BHHHHfff', TYPE_PPSANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
self._cf.send_packet(pk)
if __name__ == '__main__':
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index 9dd75d26b0925cecb259b967c253eb7d0ef2c046..71d3eb1f915fc239dbf48949a5b555afdc5533c1 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -1 +1 @@
-8,PPS_CF04,0/24/2M,0,-0.33,-1.3,-0.2,1.39,0.34,2
+6,PPS_CF04,0/24/2M/E7E7E7E704,0,-1.18,-0.65,-0.2,0.93,1.04,2
diff --git a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp b/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
index 233cb9ffa44a6d69479f3c3f407b985667384d7e..6a3fe81157a80e615719284d78cd4bc6f7cee4e1 100755
--- a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
@@ -27,7 +27,7 @@
#include "d_fall_pps/Controller.h"
#define PI 3.1415926535
-#define RATE_CONTROLLER 0
+#define RATE_CONTROLLER 7
using namespace d_fall_pps;
diff --git a/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp b/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp
index 62c89ddea729fba9ea4aeb5a014f2ca873ad887a..1a90940541e467478cd608b4bc230ded1666e71e 100755
--- a/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp
+++ b/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp
@@ -27,7 +27,7 @@
#include "d_fall_pps/Controller.h"
#define PI 3.1415926535
-#define RATE_CONTROLLER 0
+#define RATE_CONTROLLER 7
using namespace d_fall_pps;
diff --git a/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp b/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
index 1e097efdd9eee649825a1800f4cf7b2757c221ad..804618dc76395d1fca25ce4d9074913a5c5ddb2e 100644
--- a/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
+++ b/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
@@ -27,7 +27,7 @@
#include "d_fall_pps/Controller.h"
#define PI 3.1415926535
-#define RATE_CONTROLLER 0
+#define RATE_CONTROLLER 7
using namespace d_fall_pps;
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index 1ea1f35e1aeae344b89bf389084aa7f08addf6f3..de0ce9c40713a532f24c074e657ce660c435d105 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -31,7 +31,7 @@
#include <std_msgs/Int32.h>
#define PI 3.1415926535
-#define RATE_CONTROLLER 0
+#define RATE_CONTROLLER 7
using namespace d_fall_pps;