text: # -*- coding: utf-8 -*-
  #
  #     ||          ____  _ __
  #  +------+      / __ )(_) /_______________ _____  ___
  #  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
  #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  #
  #  Copyright (C) 2017 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.
  """
  Version of the AutonomousSequence.py example connecting to 2 Crazyflies and
  flying them throught a list of setpoints at the same time. This shows how to
  control more than one Crazyflie autonomously.
  """
  import time
  
  import cflib.crtp
  from cflib.crazyflie.log import LogConfig
  from cflib.crazyflie.swarm import Swarm
  from cflib.crazyflie.syncLogger import SyncLogger
  
  # Change uris and sequences according to your setup
  URI0 = 'radio://0/70/2M'
  URI1 = 'radio://0/90/2M'
  
  uris = {URI0, URI1}
  
  #             x    y    z  YAW
  sequence0 = [(2.5, 2.5, 1.0, 0),
               (2.5, 2.3, 1.0, 0),
               (2.0, 2.3, 1.0, 0),
               (2.0, 2.5, 1.0, 0),
               (2.0, 2.5, 0.5, 0)]
  
  sequence1 = [(2.0, 2.5, 1.0, 0),
               (2.0, 2.7, 1.0, 0),
               (2.5, 2.7, 1.0, 0),
               (2.5, 2.5, 1.0, 0),
               (2.5, 2.5, 0.5, 0)]
  
  seq_args = {
      URI0: [sequence0],
      URI1: [sequence1],
  }
  
  
  def wait_for_position_estimator(scf):
      print('Waiting for estimator to find position...')
  
      log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
      log_config.add_variable('kalman.varPX', 'float')
      log_config.add_variable('kalman.varPY', 'float')
      log_config.add_variable('kalman.varPZ', 'float')
  
      var_y_history = [1000] * 10
      var_x_history = [1000] * 10
      var_z_history = [1000] * 10
  
      threshold = 0.001
  
      with SyncLogger(scf, log_config) as logger:
          for log_entry in logger:
              data = log_entry[1]
  
              var_x_history.append(data['kalman.varPX'])
              var_x_history.pop(0)
              var_y_history.append(data['kalman.varPY'])
              var_y_history.pop(0)
              var_z_history.append(data['kalman.varPZ'])
              var_z_history.pop(0)
  
              min_x = min(var_x_history)
              max_x = max(var_x_history)
              min_y = min(var_y_history)
              max_y = max(var_y_history)
              min_z = min(var_z_history)
              max_z = max(var_z_history)
  
              # print("{} {} {}".
              #       format(max_x - min_x, max_y - min_y, max_z - min_z))
  
              if (max_x - min_x) < threshold and (
                      max_y - min_y) < threshold and (
                      max_z - min_z) < threshold:
                  break
  
  
  def reset_estimator(scf):
      cf = scf.cf
      cf.param.set_value('kalman.resetEstimation', '1')
      time.sleep(0.1)
      cf.param.set_value('kalman.resetEstimation', '0')
  
      wait_for_position_estimator(cf)
  
  
  def position_callback(timestamp, data, logconf):
      x = data['kalman.stateX']
      y = data['kalman.stateY']
      z = data['kalman.stateZ']
      print('pos: ({}, {}, {})'.format(x, y, z))
  
  
  def start_position_printing(scf):
      log_conf = LogConfig(name='Position', period_in_ms=500)
      log_conf.add_variable('kalman.stateX', 'float')
      log_conf.add_variable('kalman.stateY', 'float')
      log_conf.add_variable('kalman.stateZ', 'float')
  
      scf.cf.log.add_config(log_conf)
      log_conf.data_received_cb.add_callback(position_callback)
      log_conf.start()
  
  
  def run_sequence(scf, sequence):
      cf = scf.cf
  
      cf.param.set_value('flightmode.posSet', '1')
  
      for position in sequence:
          print('Setting position {}'.format(position))
          for i in range(50):
              cf.commander.send_setpoint(position[1], position[0],
                                         position[3],
                                         int(position[2] * 1000))
              time.sleep(0.1)
  
      cf.commander.send_setpoint(0, 0, 0, 0)
      # Make sure that the last packet leaves before the link is closed
      # since the message queue is not flushed before closing
      time.sleep(0.1)
  
  
  if __name__ == '__main__':
      cflib.crtp.init_drivers(enable_debug_driver=False)
  
      with Swarm(uris) as swarm:
          swarm.parallel(reset_estimator)
          # swarm.parallel(start_position_printing)
          swarm.parallel(run_sequence, args_dict=seq_args)
  