Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # -*- coding: utf-8 -*-
- #
- # || ____ _ __
- # +------+ / __ )(_) /_______________ _____ ___
- # | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
- # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
- # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
- #
- # Copyright (C) 2019 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, see <https://www.gnu.org/licenses/>.
- """
- Simple example that connects to one crazyflie, sets the initial position/yaw
- and flies a trajectory.
- The initial pose (x, y, z, yaw) is configured in a number of variables and
- the trajectory is flown relative to this position, using the initial yaw.
- This example is intended to work with any absolute positioning system.
- It aims at documenting how to take off with the Crazyflie in an orientation
- that is different from the standard positive X orientation and how to set the
- initial position of the kalman estimator.
- """
- import math
- import time
- import cflib.crtp
- from cflib.crazyflie import Crazyflie
- from cflib.crazyflie.log import LogConfig
- from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
- from cflib.crazyflie.syncLogger import SyncLogger
- from cflib.utils import uri_helper
- # URI to the Crazyflie to connect to
- uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E8')
- # Change the sequence according to your setup
- # x y z
- sequence = [
- (1, 1, 1.0),
- (1, 1, 1.0),
- ]
- 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 set_initial_position(scf, x, y, z, yaw_deg):
- scf.cf.param.set_value('kalman.initialX', x)
- scf.cf.param.set_value('kalman.initialY', y)
- scf.cf.param.set_value('kalman.initialZ', z)
- yaw_radians = math.radians(yaw_deg)
- scf.cf.param.set_value('kalman.initialYaw', yaw_radians)
- 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 run_sequence(scf, sequence, base_x, base_y, base_z, yaw):
- cf = scf.cf
- for position in sequence:
- print('Setting position {}'.format(position))
- x = position[0] + base_x
- y = position[1] + base_y
- z = position[2] + base_z
- for i in range(50):
- cf.commander.send_position_setpoint(x, y, z, yaw)
- time.sleep(0.1)
- cf.commander.send_stop_setpoint()
- # 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()
- # Set these to the position and yaw based on how your Crazyflie is placed
- # on the floor
- initial_x = 1.0
- initial_y = 1.0
- initial_z = 0.0
- initial_yaw = 90 # In degrees
- # 0: positive X direction
- # 90: positive Y direction
- # 180: negative X direction
- # 270: negative Y direction
- with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
- set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw)
- reset_estimator(scf)
- run_sequence(scf, sequence,
- initial_x, initial_y, initial_z, initial_yaw)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement