implementing motion commander into param.py example

Firmware/software/electronics/mechanics
Post Reply
alex236
Beginner
Posts: 5
Joined: Tue Mar 13, 2018 8:19 pm

implementing motion commander into param.py example

Post by alex236 »

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!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: implementing motion commander into param.py example

Post by arnaud »

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.
alex236
Beginner
Posts: 5
Joined: Tue Mar 13, 2018 8:19 pm

Re: implementing motion commander into param.py example

Post by alex236 »

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)
There is no errors so far, the crazyflie is able to take off. However, it is not printing out the parameter table with all current values. I am not sure why it is not printing out the values to the terminal.

Essentially, we want to be able to change pid_rate yaw values while in flight, that is the ultimate goal.
Post Reply