Commit 94ff6027 authored by roangel's avatar roangel
Browse files

fixed some thing in python script

parent 1c914c48
......@@ -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)
......
......@@ -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__':
......
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
......@@ -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;
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment