Hello everyone!,
I was wondering if anyone has tried to implement motion commander into the basicparam example in the python library.
I tried to implement the code but it doesn't not seem to work at all. It would always give an error when trying to implement it.
Please let me know !
Thanks!
implementing motion commander into param.py example
Re: implementing motion commander into param.py example
What are you trying to achieve?
I think you should take it the other way around: start from the motion commander example and add the required param read/write. Then If you tell us more precisely the error you are getting it would help.
I think you should take it the other way around: start from the motion commander example and add the required param read/write. Then If you tell us more precisely the error you are getting it would help.
Re: implementing motion commander into param.py example
Code: Select all
# -*- 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.
"""
This script shows the basic use of the MotionCommander class.
Simple example that connects to the crazyflie at `URI` and runs a
sequence. This script requires some kind of location system, it has been
tested with (and designed for) the flow deck.
Change the URI variable to your Crazyflie configuration.
"""
import logging
import time
import random
import matlab.engine
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
#from CrazymothLogger import *
#URI = 'radio://0/80/250k'
#URI = cflib.crtp.scan_interfaces()[0][0]
URI = 'radio://0/80/250K'
#eng=matlab.engine.start_matlab()
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
log_vars=['Sensor.gas', 'kalman.stateX','kalman.stateY', 'kalman.stateZ']
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 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)
def _connected(self, link_uri):
self._param_groups = []
self._param_check_list = []
random.seed()
p_toc = self._cf.param.toc.toc
for group in sorted(p_toc.keys()):
print('{}'.format(group))
for param in sorted(p_toc[group].keys()):
print('\t{}'.format(param))
self._param_check_list.append('{0}.{1}'.format(group, param))
self._cf.param.add_update_callback(group=group, name=None,
cb=self._param_callback)
def _param_callback(self,name,value):
print ('{0}:{1}'.format(name,value))
self._param_check_list.remove(name)
if len(self._param_check_list) == 0:
print('Have fetched all parameter values')
for g in self._param_groups:
self._cf.param.remove_update_callback(group=g,
cb=self._param_callback)
pkd = random.random()
print('')
print('Write: pid_attitude.pitch_kd={:.2f}'.format(pkd))
self._cf.param.add_update_callback(group='pid_attitude',
name='pitch_kd',
cb=self._a_pitch_kd_callback)
self._cf.param.set_value('pid_attitude.pitch_kd',
'{:.2f}'.format(pkd))
def _a_pitch_kd_callback(self,name,value):
print('Readback: {0}={1}'.format(name,value))
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
# We take off when the commander is created
arr = None
with MotionCommander(scf, default_height=0.3) as mc:
time.sleep(2)
Essentially, we want to be able to change pid_rate yaw values while in flight, that is the ultimate goal.