From 94ff6027fa5a0c2b1668fbaf0365af97ffacb824 Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Fri, 13 Oct 2017 12:10:24 +0200
Subject: [PATCH] fixed some thing in python script
---
.../src/d_fall_pps/crazyradio/CrazyRadio.py | 44 ++++++++++++++++---
pps_ws/src/d_fall_pps/crazyradio/TestCF.py | 2 +-
pps_ws/src/d_fall_pps/param/Crazyflie.db | 2 +-
.../src/CircleControllerService.cpp | 2 +-
.../d_fall_pps/src/FollowCrazyflieService.cpp | 2 +-
.../src/d_fall_pps/src/FollowN_1Service.cpp | 2 +-
.../d_fall_pps/src/SafeControllerService.cpp | 2 +-
7 files changed, 43 insertions(+), 13 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index dd2b8629..62da8744 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 2682260e..c5fa4f44 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 9dd75d26..71d3eb1f 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 233cb9ff..6a3fe811 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 62c89dde..1a909405 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 1e097efd..804618dc 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 1ea1f35e..de0ce9c4 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;
--
GitLab