Commit 0ea587d5 authored by michaero's avatar michaero
Browse files

Logging from CF: Added functions, changed names, added second logging for position and attitude

parent 8a135234
......@@ -153,19 +153,45 @@ class PPSRadioClient:
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _data_received_callback(self, timestamp, data, logconf):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
def _xyzrpy_received_callback(self):
#coordX = Float32()
#coordY = Float32()
#coordZ = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
#coordX.data = data["xxx.x"]
#coordY.data = data["xxx.y"]
#coordY.data = data["xxx.y"]
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
stabilizerRoll.data = data["stabilizer.roll"]
#TODO: Add Publisher
print "roll: %s" % stabilizerRoll
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)
#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))
......@@ -174,28 +200,76 @@ class PPSRadioClient:
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
#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 _init_logging(self):
def _start_logging(self):
self.logconf = LogConfig("LoggingTest", 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"
def init_log_battery(self):
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)
self.batLog.error_cb.add_callback(self._vbat_logging_error)
print "batLog valid"
else:
print "batLog invalid"
self.batLog.start()
print "batLog start"
def init_log_xyzrpy(self):
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");
self.xyzrpy.add_variable("stabilizer.roll", "float");
self.xyzrpy.add_variable("stabilizer.pitch", "float");
self.xyzrpy.add_variable("stabilizer.yaw", "float");
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)
print "xyzrpy valid"
else:
print "logconf invalid"
print "xyzrpy invalid"
self.xyzrpy.start()
print "xyzrpy start"
def _start_logging(self):
self.init_log_battery()
self.logconf.start()
print "logconf start"
#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):
"""
......@@ -233,10 +307,17 @@ class PPSRadioClient:
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
self.logconf.stop()
rospy.loginfo("logconf stopped")
self.logconf.delete()
rospy.loginfo("logconf deleted")
#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()
rospy.loginfo("batLog deleted")
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
......
......@@ -4,5 +4,6 @@ strictSafety: true
angleMargin: 0.6
battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 100 # in ms
battery_polling_period: 500 # in ms
xyzrpy_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