Commit 8db3ae7b authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

added functioning additional data logger for stabilizer data (roll, pitch, yaw), prepared for xyz

parent 4fb71e3b
......@@ -153,10 +153,11 @@ class PPSRadioClient:
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _xyzrpy_received_callback(self):
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
#coordX = Float32()
#coordY = Float32()
#coordZ = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
......@@ -173,26 +174,15 @@ class PPSRadioClient:
print "pitch: %s" % stabilizerPitch
print "yaw: %s" %stabilizerYaw
#TODO: Test, then clean up
#def _data_received_callback(self, timestamp, data, logconf):
def _vbat_received_callback(self, timestamp, data, batLog):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
#stabilizerYaw = Float32()
#stabilizerPitch = Float32()
#stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
#stabilizerYaw.data = data["stabilizer.yaw"]
#stabilizerPitch.data = data["stabilizer.pitch"]
#stabilizerRoll.data = data["stabilizer.roll"]
bag.write('batteryVoltage', batteryVolt)
#bag.write('stabilizerYaw', stabilizerYaw)
#bag.write('stabilizerPitch', stabilizerPitch)
#bag.write('stabilizerRoll', stabilizerRoll)
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
print "batteryVolt: %s" % batteryVolt
......@@ -212,9 +202,13 @@ class PPSRadioClient:
# def _init_logging(self):
def init_log_battery(self):
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
self.batLog.add_variable("pm.vbat", "float");
self._cf.log.add_config(self.batLog)
if self.batLog.valid:
self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
......@@ -227,7 +221,11 @@ class PPSRadioClient:
print "batLog start"
def init_log_xyzrpy(self):
self.xyzrpy = LogConfig("XYZRPYLog", 50)
global xyzrpy_polling_period
xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")
self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
#self.xyzrpy.add_variable("xxx.x", "float");
#self.xyzrpy.add_variable("xxx.y", "float");
#self.xyzrpy.add_variable("xxx.z", "float");
......@@ -253,27 +251,6 @@ class PPSRadioClient:
self.init_log_xyzrpy() #TODO: Test
#TODO: Test, then clean up
#self.logconf = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
#self.logconf.add_variable("stabilizer.roll", "float");
#self.logconf.add_variable("stabilizer.pitch", "float");
#self.logconf.add_variable("stabilizer.yaw", "float");
#self.logconf.add_variable("pm.vbat", "float");
#self._cf.log.add_config(self.logconf)
#if self.logconf.valid:
# self.logconf.data_received_cb.add_callback(self._data_received_callback)
# self.logconf.error_cb.add_callback(self._logging_error)
# print "logconf valid"
#else:
# print "logconf invalid"
#self.logconf.start()
#print "logconf start"
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
......@@ -310,13 +287,6 @@ class PPSRadioClient:
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
#TODO: Test, then clean up
#self.logconf.stop()
#rospy.loginfo("logconf stopped")
#self.logconf.delete()
#rospy.loginfo("logconf deleted")
self.batLog.stop()
rospy.loginfo("batLog stopped")
self.batLog.delete()
......@@ -407,9 +377,7 @@ if __name__ == '__main__':
#use this following two lines to connect without data from CentralManager
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
......
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