fixed some thing in python script

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)
 
 
 

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__':

pps_ws/src/d_fall_pps/param/Crazyflie.db

-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

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;
 

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;
 

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;
 

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;