Code: Select all
# -*- coding: utf-8 -*-
import sys
sys.path.append("../lib")
import cflib.crtp
import logging
import time
import datetime
from threading import Timer
import math
import matlab.engine
eng = matlab.engine.start_matlab()
import cflib.crtp
from cfclient.utils.logconfigreader import LogConfig
from cflib.crazyflie import Crazyflie
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
file1 = open("IMU.txt","w")
file2 = open("Vicon.txt","w")
file3 = open("U vector.txt","w")
##file4 = open("Vicon_Position.txt","w")
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
## Thread(target=self._ramp_motors).start()
####
## Adding a separate log config for accelerometer data
self._lg_stab1 = LogConfig(name="Stabilizer", period_in_ms=20)
self._lg_stab1.add_variable("gyro.x", "float")
self._lg_stab1.add_variable("gyro.y", "float")
self._lg_stab1.add_variable("gyro.z", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab1)
# This callback will receive the data
self._lg_stab1.data_received_cb.add_callback(self._stab1_log_data)
le._cf.commander.send_setpoint(0, 0, 0, 0)
# This callback will be called on errors
self._lg_stab1.error_cb.add_callback(self._stab1_log_error)
# Start the logging
self._lg_stab1.start()
except KeyError as e:
print "Could not start log configuration," \
"{} not found in TOC".format(str(e))
except AttributeError:
print "Could not add Stabilizer log config, bad configuration."
####
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=20)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("acc.x", "float")
self._lg_stab.add_variable("acc.y", "float")
self._lg_stab.add_variable("acc.z", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
le._cf.commander.send_setpoint(0, 0, 0, 0)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print "Could not start log configuration," \
"{} not found in TOC".format(str(e))
except AttributeError:
print "Could not add Stabilizer log config, bad configuration."
# Start a timer to disconnect in 10s
t = Timer(15, self._cf.close_link)
t.start()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print "Error when logging %s: %s" % (logconf.name, msg)
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
## acc_x = (data["acc.x"]+0.0039)* 9.807
## acc_y = ( data["acc.y"] - 0.00063739)*9.807
## acc_z = (data["acc.z"] - 0.9871)*9.807
global pos_x_old, pos_y_old , pos_z_old , Time1, MS1_past,Time2, MS2_past, P_est_past, u
global ep_int_past ,ea_int_past
## global pos_y_old
## global pos_z_old
acc_x = (data["acc.x"])* 9.871
acc_y = ( data["acc.y"])*9.871
acc_z = (data["acc.z"] - 0.9871)*9.871
roll = data["stabilizer.roll"]
pitch = data["stabilizer.pitch"]
yaw = data["stabilizer.yaw"]
time = str(datetime.datetime.now().time())
time = time.split('.')
MS1 = float(time[1])
Dt = (MS1 - MS1_past)/1000000
if Dt < 0:
Dt = 1+ Dt
if Time2 == 0:
Dt = 0
Time1 = Time1 + Dt
file1.write(str(pitch) + "\t" + str(roll) + "\t" + str(yaw)+ "\t" + "\t" + str(acc_x) + "\t" + str(acc_y) + "\t" + str(acc_z)+ "\t" + "\t" + str(self.gyro_x) + "\t" + str(self.gyro_y) + "\t" + str(self.gyro_z)+ "\t" + str(time[0]) + "\t" + str(time[1]) + "\t" + str(Time1)+ "\t" + str(Dt)+ "\n")
## print "Roll is %.3f Pitch is %.3f Yaw is %.3f\n" % (roll,pitch,yaw)
## print "Acc_x is %.5f Acc_y is %.3f Acc_z is %.3f\n" % (acc_x,acc_y,acc_z)
## print "Roll_omega is %.3f Pitch_omega is %.3f Yaw_omega is %.3f\n" % (self.gyro_y,self.gyro_x,self.gyro_z)
MS1_past = MS1
#q_est_past = [pos_x_old,pos_y_old,pos_z_old, roll,pitch,yaw,vel_x,vel_y,vel_z,self.omega_x,self.omega_y,self.omega_z]
(pos_x, pos_y, pos_z,vel_x,vel_y,vel_z,RPM,roll_des,pitch_des,yaw_des,P_est, ep_int ,ea_int ) = eng.Control_variables_test_9_2(pos_x_old,pos_y_old,pos_z_old, roll,pitch,yaw,self.gyro_x,self.gyro_y,self.gyro_z,acc_x,acc_y,acc_z,P_est_past,Time2,Dt,ep_int_past,ea_int_past,nargout=13)
print "X is %.3f Y is %.3f Z is %.3f\n" % (pos_x,pos_y,pos_z)
## print "V_X is %.3f V_Y is %.3f V_Z is %.3f\n" % (vel_x,vel_y,vel_z)
print RPM,roll_des,pitch_des,yaw_des
print Time2
time = str(datetime.datetime.now().time())
time = time.split('.')
MS2 = float(time[1])
Dt = (MS2 - MS1)/1000000
if Dt < 0:
Dt = 1+ Dt
Time2 = Time2 + Dt
file2.write(str(pos_x) + "\t" + str(pos_y) + "\t" + str(pos_z) + "\t" + "\t" + str(vel_x) + "\t"+ str(vel_y) + "\t" + str(vel_z) + "\t" + "\t" + str(time[0]) + "\t" + str(time[1])+ "\t" + str(Time2) + "\n")
file3.write(str(RPM) + "\t" + str(roll_des) + "\t" + str(pitch_des) + "\t" + str(yaw_des) + "\t" + str(Time2) + "\n")
le._cf.commander.send_setpoint(roll_des, pitch_des, yaw_des, RPM)
## le._cf.commander.send_setpoint(0, 0, 0, 38000)
pos_x_old = pos_x
pos_y_old = pos_y
pos_z_old = pos_z
MS2_past = MS2
P_est_past = P_est
ep_int_past = ep_int
ea_int_past = ea_int
## Time2 = Time2 + Dt
def _stab1_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print "Error when logging %s: %s" % (logconf.name, msg)
def _stab1_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
self.gyro_y = data["gyro.y"]
self.gyro_x = data["gyro.x"]
self.gyro_z = data["gyro.z"]
#print "Roll_omega is %.3f Pitch_omega is %.3f Yaw_omega is %.3f\n" % (gyro_y,gyro_x,gyro_z)#inertial_fram
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print "Connection to %s failed: %s" % (link_uri, msg)
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print "Disconnected from %s" % link_uri
file1.close()
file2.close()
file3.close()
eng.F_Vicon_stop(nargout=0)
self.is_connected = False
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print "Scanning interfaces for Crazyflies..."
available = cflib.crtp.scan_interfaces()
print "Crazyflies found:"
for i in available:
print i[0]
## eng.figure(nargout=0)
pos_x_old = 0
pos_y_old = 0
pos_z_old = 0
P_est_past = 0
Time1 = 0
Time2 = 0
MS1_past = 0
MS2_past = 0
ep_int_past = matlab.double([0,0])
ea_int_past = matlab.double([0,0,0,0])
u = 0
eng.F_ViconInitialization(nargout=0)
le = LoggingExample(available[0][0])
else:
print "No Crazyflies found, cannot run example"
# The Crazyflie lib doesn't contain anything to keep the application alive,
# so this is where your application should do something. In our case we
# are just waiting until we are disconnected.
while le.is_connected:
time.sleep(1)