Commit 9faf465f authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

added logging for anchor distances, not yet tested

parent 8db3ae7b
......@@ -153,6 +153,23 @@ class PPSRadioClient:
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distance1 = Float32()
distance2 = Float32()
distance3 = Float32()
distance4 = Float32()
distance5 = Float32()
distance6 = Float32()
distance1.data = data["ranging.distance1"]
distance2.data = data["ranging.distance2"]
distance3.data = data["ranging.distance3"]
distance4.data = data["ranging.distance4"]
distance5.data = data["ranging.distance5"]
distance6.data = data["ranging.distance6"]
#TODO: Add Publisher
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
#coordX = Float32()
#coordY = Float32()
......@@ -170,9 +187,10 @@ class PPSRadioClient:
stabilizerRoll.data = data["stabilizer.roll"]
#TODO: Add Publisher
print "roll: %s" % stabilizerRoll
print "pitch: %s" % stabilizerPitch
print "yaw: %s" %stabilizerYaw
#print "roll: %s" % stabilizerRoll
#print "pitch: %s" % stabilizerPitch
#print "yaw: %s" %stabilizerYaw
#def _data_received_callback(self, timestamp, data, logconf):
def _vbat_received_callback(self, timestamp, data, batLog):
......@@ -185,19 +203,13 @@ class PPSRadioClient:
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
print "batteryVolt: %s" % batteryVolt
#print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
#def _logging_error(self, logconf, msg):
# print "Error when logging %s" % logconf.name
def _vbat_logging_error(self, batLog, msg):
print "Error when logging %s" % batLog.name
def _xyzrpy_logging_error(self, xyzrpy, msg):
print "Error when logging %s" % xyzrpy.name
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
# def _init_logging(self):
......@@ -212,7 +224,7 @@ class PPSRadioClient:
self._cf.log.add_config(self.batLog)
if self.batLog.valid:
self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
self.batLog.error_cb.add_callback(self._vbat_logging_error)
self.batLog.error_cb.add_callback(self._logging_error)
print "batLog valid"
else:
print "batLog invalid"
......@@ -236,7 +248,7 @@ class PPSRadioClient:
self._cf.log.add_config(self.xyzrpy)
if self.xyzrpy.valid:
self.xyzrpy.data_received_cb.add_callback(self._xyzrpy_received_callback)
self.xyzrpy.error_cb.add_callback(self._xyzrpy_logging_error)
self.xyzrpy.error_cb.add_callback(self._logging_error)
print "xyzrpy valid"
else:
print "xyzrpy invalid"
......@@ -244,12 +256,37 @@ class PPSRadioClient:
self.xyzrpy.start()
print "xyzrpy start"
def init_log_anchordist(self):
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorDist = LogConfig("AnchorDistances", anchordist_polling_period)
self.anchorDist.add_variable("ranging.distance1","float");
self.anchorDist.add_variable("ranging.distance2","float");
self.anchorDist.add_variable("ranging.distance3","float");
self.anchorDist.add_variable("ranging.distance4","float");
self.anchorDist.add_variable("ranging.distance5","float");
self.anchorDist.add_variable("ranging.distance6","float");
self._cf.log.add_config(self.anchorDist)
if self.anchorDist.valid:
self.anchorDist.data_received_cb.add_callback(self._anchorDist_received_callback)
self.anchorDist.error_cb.add_callback(self._logging_error)
print "anchorDist valid"
else:
print "anchorDist invalid"
self.anchorDist.start()
print "anchorDist start"
def _start_logging(self):
self.init_log_battery()
self.init_log_xyzrpy() #TODO: Test
self.init_log_xyzrpy()
#self.init_log_anchordist() #TODO: test
def _connected(self, link_uri):
"""
......
......@@ -6,4 +6,4 @@ battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 500 # in ms
xyzrpy_polling_period: 50
anchordist_polling_period: 50
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