Commit c75f4a5a authored by muelmarc's avatar muelmarc
Browse files

Crazyradio logs batterz status and writes to bag file. Conversion issue and...

Crazyradio logs batterz status and writes to bag file. Conversion issue and bagdata is useless. But battery status and stabilizer logs are already printed to console at 1000ms rate
parent 8cc10520
......@@ -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]
......@@ -89,7 +89,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