Commit ca9a7fc5 authored by bucyril's avatar bucyril
Browse files

Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into pps_project

parents 66921fc2 c75f4a5a
......@@ -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)
......
teamName: "Two"
crazyFlieName: "cfFour"
crazyFlieAddress: "radio://0/99/2M"
crazyFlieName: "cfTwo"
crazyFlieAddress: "radio://0/69/2M"
safeController: "SafeControllerService/RateController"
customController: ""
......@@ -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]
......@@ -91,7 +91,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;
......
......@@ -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;
......
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