# -*- coding: utf-8 -*- # # || ____ _ __ # +------+ / __ )(_) /_______________ _____ ___ # | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # # Copyright (C) 2014 Bitcraze AB # # Crazyflie Nano Quadcopter Client # # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU General Public License # as published by the Free Software Foundation; either version 2 # of the License, or (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, # MA 02110-1301, USA. # from mako.runtime import Undefined """ Simple example that connects to the first Crazyflie found, logs data that you choose and plot it. """ import logging import math import time import matplotlib.pyplot as plt from threading import Timer from threading import Thread import movement import cflib.crtp # noqa from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) class Logger: """ Simple logging example class that logs the Stabilizer from a supplied link uri and disconnects after 5s. """ def __init__(self, period): """ Initialize and run the example with the specified link_uri """ self.trig = 0 self._cf = Crazyflie(rw_cache='./cache') self.dati_acquisiti = False # diventa vera dopo aver scelto i dati da loggare self.dati = {} # dizionario vuoto che assocerà ad ogni chiave (parametro) una lista di numeri (dati) self.diz = {} # dizionario in cui le chiavi hanno '_' al posto del '.' (necessario per i salvataggi) # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() print('Crazyflies found:') j = 0 for i in available: print('Drone ', j, ': ', i[0]) j = j + 1 numdrone = input('Inserisci il numero del drone: ') if numdrone == "exit": exit('Programma arrestato') if len(available) > 0: link_uri = available[int(numdrone)][0] else: print('No Crazyflies found, cannot run example') # Connect some callbacks from the Crazyflie API scf = SyncCrazyflie(link_uri, self._cf) self.cf = scf.cf 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 self.calc_offset = True def crazyflie(self): return self._cf def close_link(self): self._cf.close_link() def set_trig(self): self.trig = 1 time.sleep(0.01) self.trig = 0 # def set_trig_dwn(self): # self.trig=0 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) # The definition of the logconfig can be made before connecting self._lg1 = LogConfig(name='Block1', period_in_ms=30) # period_in_ms = float(period) self._lg2 = LogConfig(name='Block2', period_in_ms=30) parametri = {'acc.x': 'ax', 'acc.y': 'ay', 'acc.z': 'az', 'controller.actuatorThrust': 'caT', 'controller.pitch': 'cp', 'controller.roll': 'cr', 'controller.yaw': 'cy', 'ctrltarget.pitch': 'ctp', 'ctrltarget.roll': 'ctr', 'ctrltarget.yaw': 'cty', 'gyro.x': 'gx', 'gyro.y': 'gy', 'gyro.z': 'gz', 'motor.m1': 'm1', 'motor.m2': 'm2', 'motor.m3': 'm3', 'motor.m4': 'm4', 'pid_attitude.pitch_outD': 'apoD', 'pid_attitude.pitch_outI': 'apoI', 'pid_attitude.pitch_outP': 'apoP', 'pid_attitude.roll_outD': 'aroD', 'pid_attitude.roll_outI': 'aroI', 'pid_attitude.roll_outP': 'aroP', 'pid_attitude.yaw_outD': 'ayoD', 'pid_attitude.yaw_outI': 'ayoI', 'pid_attitude.yaw_outP': 'ayoP', 'pid_rate.pitch_outD': 'rpoD', 'pid_rate.pitch_outI': 'rpoI', 'pid_rate.pitch_outP': 'rpoP', 'pid_rate.roll_outD': 'rroD', 'pid_rate.roll_outI': 'rroI', 'pid_rate.roll_outP': 'rroP', 'pid_rate.yaw_outD': 'ryoD', 'pid_rate.yaw_outI': 'ryoI', 'pid_rate.yaw_outP': 'ryoP', 'stabilizer.thrust': 'st', 'stabilizer.pitch': 'sp', 'stabilizer.roll': 'sr', 'stabilizer.yaw': 'sy', 'range.zrange': 'z'} print('\n') for chiave in parametri.keys(): print(chiave + ': ' + parametri[chiave] + '\n') codici = input('\nDigitare i codici che corrispondono ai dati che vuoi loggare separati da uno spazio.' '\n** N.B. Inserire lo spazio anche alla fine: \n') lista = [] posizione = 0 for lettera in codici: if lettera == ' ': pos = codici.find(' ', posizione) lista.append(codici[posizione:pos]) posizione = pos + 1 self.dati_acquisiti = True i = 0 for j in parametri.keys(): if parametri[j] in lista: i = i + 1 self.dati[j] = [] nuova = j.replace('.', '_') self.diz[nuova] = [] if i < 5 : self._lg1.add_variable(j, 'float') else: self._lg2.add_variable(j, 'float') # self._lg3 = LogConfig(name='Block3', period_in_ms=float(period)) # self._lg4 = LogConfig(name='Block4', period_in_ms=100) # self._lg5 = LogConfig(name='Block5', period_in_ms=float(period)) # Euler Angles # self._lg.add_variable('stabilizer.roll', 'float') # self._lg.add_variable('stabilizer.pitch', 'float') # self._lg.add_variable('stabilizer.yaw', 'float') # Quaternions # self._lg1.add_variable('kalman.q0', 'float') # self._lg1.add_variable('kalman.q1', 'float') # self._lg1.add_variable('kalman.q2', 'float') # self._lg1.add_variable('kalman.q3', 'float') # Accelerazioni # self._lg1.add_variable('acc.x', 'float') # self._lg1.add_variable('acc.y', 'float') # self._lg1.add_variable('acc.z', 'float') # self._lg1.add_variable('acc.x', 'float') # self._lg1.add_variable('acc.y', 'float') # self._lg1.add_variable('acc.z', 'float') # self._lg1.add_variable('gyro.x', 'float') # self._lg1.add_variable('gyro.y', 'float') # self._lg1.add_variable('gyro.z', 'float') # self._lg2.add_variable('motor.m1', 'float') # self._lg2.add_variable('motor.m2', 'float') # self._lg2.add_variable('motor.m3', 'float') # self._lg2.add_variable('motor.m4', 'float') # self._lg2.add_variable('range.zrange', '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._lg1) self._cf.log.add_config(self._lg2) # This callback will receive the data self._lg1.data_received_cb.add_callback(self.log_data_block1) self._lg2.data_received_cb.add_callback(self.log_data_block2) # This callback will be called on errors self._lg1.error_cb.add_callback(self.log_error) self._lg2.error_cb.add_callback(self.log_error) # Start the logging self._lg1.start() self._lg2.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.') Thread(target=self._ramp_motors).start() # Start a timer to disconnect in 10s # t = Timer(5, self._cf.close_link) # t.start() def log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def log_data_block1(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" # print('[%s]: %s' % (logconf.name, data)) #OSS: data è un dizionario for chiave in data.keys(): nuova = chiave.replace('.', '_') self.diz[nuova].append(data[chiave]) self.dati[chiave].append(data[chiave]) # estimate = open('estimate' + period + 'prova' + numero + '.txt', 'a') # write = estimate.write(str(timestamp) + ',' + str(data) + '\n') # estimate.closed def log_data_block2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" # print('[%s]: %s' % (logconf.name, data)) for chiave in data.keys(): nuova = chiave.replace('.','_') self.diz[nuova].append(data[chiave]) self.dati[chiave].append(data[chiave]) # print('[%s]: %s' % (logconf.name, data)) ####### CREAZIONE DEL FILE ######### # estimate = open('estimate' + period + 'prova' + numero + '.txt', 'a') # write = estimate.write(str(timestamp) + ',' + str(data) + '\n') # estimate.closed def log_data_block3(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print('[%s]: %s' % (logconf.name, data)) estimate = open('estimate' + period + 'prova' + numero + '.txt', 'a') write = estimate.write(str(timestamp) + ',' + str(data) + '\n') estimate.closed def log_data_block4(self, timestamp, data, logconf): print('[%s]: %s' % (logconf.name, data)) estimate = open('estimate' + period + 'prova' + numero + '.txt', 'a') write = estimate.write(str(timestamp) + ',' + str(data) + '\n') estimate.closed self.varX = data['kalman.varPX'] self.varY = data['kalman.varPY'] self.varZ = data['kalman.varPZ'] def log_data_block5(self, timestamp, data, logconf): print('[%s]: %s' % (logconf.name, data)) estimate = open('estimate' + period + 'prova' + numero + '.txt', 'a') write = estimate.write(str(timestamp) + ',' + str(data) + '\n') estimate.closed self.rollRate = data['motor.m1'] self.pitchRate = data['motor.m2'] self.yawRate = data['motor.m3'] self.actuatorThrust = data['motor.m4'] 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) self.is_connected = False def getDati(self): return self.diz def getDatiAcquisiti(self): return self.dati_acquisiti if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) period = input('Inserire il periodo di campionamento in ms: ') numero = input('Inserire il numero della prova: ') le = Logger(period) h = 0.5 v = 1 while not (le.getDatiAcquisiti()): # finchè non acquisisci i dati: aspetta time.sleep(0.5) # volendo si può mettere 'if le.is_connected' ma non è necessario perchè se non si connette già partono delle eccezioni # le.cf è un attributo di le che è un oggetto Crazyflie le.cf.param.set_value('kalman.resetEstimation', '1') time.sleep(1) le.cf.param.set_value('kalman.resetEstimation', '0') time.sleep(1) movement.decolla(le.cf, h) movement.fermo(le.cf, h, 1) movement.atterra(le.cf, h) # ########Quadrato########## # for _ in range(5): # # le._cf.commander.send_hover_setpoint(v, 0, 0, a/b) # time.sleep(0.2) # # for _ in range(45): # le._cf.commander.send_hover_setpoint(0, 0, 20, a/b) # time.sleep(0.1) # # for _ in range(5): # le._cf.commander.send_hover_setpoint(v, 0, 0, a/b) # time.sleep(0.2) # # for _ in range(45): # le._cf.commander.send_hover_setpoint(0, 0, 20, a/b) # time.sleep(0.1) # # for _ in range(5): # le._cf.commander.send_hover_setpoint(v, 0, 0, a/b) # time.sleep(0.2) # # for _ in range(45): # le._cf.commander.send_hover_setpoint(0, 0, 20, a/b) # time.sleep(0.1) # # for _ in range(5): # le._cf.commander.send_hover_setpoint(v, 0, 0, a/b) # time.sleep(0.2) # # for _ in range(45): # le._cf.commander.send_hover_setpoint(0, 0, 20, a/b) # time.sleep(0.1) # # for _ in range(40): # le._cf.commander.send_hover_setpoint(0, 0, 0, a/b) # time.sleep(0.1) # # # ####### Cerchio ######## # for _ in range(50): # le._cf.commander.send_hover_setpoint(v, 0, 40 * 2, a/b) # time.sleep(0.1) time.sleep(0.5) dati_letti = le.getDati() le.cf.close_link() for par in dati_letti.keys(): plt.plot(dati_letti[par]) plt.title(par) plt.show() # ***** Chiudere la finestra della figura per vedere le successive