How to let the crazyflie to fly to a specific coordinate based on Mocap feedback? ?

Discussions related to positioning systems where the position is calculated outside the Crazyflie and sent to the device using radio, including MoCap systems
Post Reply
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

How to let the crazyflie to fly to a specific coordinate based on Mocap feedback? ?

Post by DarkKnight »

Hello, everyone,

I am trying to let the Crazyflie fly to a specified coordinate based on Mocap feedback and then hover at this point for a while? and currently what I have implemented is that I can use the Optitrack to receive the Crazyflie's pose information and use cf.extpos.send_extpose to send the external pose into kalman filter.

There are three scripts under the crazyflie-lib-python which might be helpful, but they seems similar and I cannot understand the difference between them.

In the directory crazyflie-lib-python/positioning/, there are
  • motion_commander.py
  • position_hl_commander.py
those two scripts both contains many high level commands, but what are differences between them? According to the introduction,my understanding is that motion_commander.py is used for open-loop control, but the position_hl_commander is used for closed-loop control, right?

In the directory crazyflie-lib-python/crazyflie/, there is
  • high_level_commander.py
what's the difference between position_hl_commander.py and high_level_commander.py?If I want my Crazyflie fly to a specified coordinate based on Mocap feedback and then hover at this point, what kind of scripts should I use?
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to let the crazyflie to fly to a specific coordinate based on Mocap feedback? ?

Post by kimberly »

Sorry for the late reply!

Let me try to help out if still necessary.

Differences between Motion commander and High level commander
It is technically all closed loop but it is mostly where the trajectory is generated and where the setpoint handling lies. The Motion commander generates the trajectory on the pc site and send each setpoint over radio. With the high level commander, it will only receive an high level command from the cflib (like take off), and onboard will handle all the trajectory and setpoint handling. The second hour of this lecture we did for EPFL explains the differences between these commanders.


the difference between position hl commander and high level commander:

Technically the HL commander and HL position commander are doing exactly the same thing, but for the HL position commander, there are some added functionality from CFLIB to make things easier. In the simple example script, you see that we send a take off command, wait for a little bit and than land again and wait for that as well. Those functions are none blocking but we always need to add a time.sleep to it to prevent the script to send another command 'too soon'. The HL position commander class actually incorporates those sleeps already in their command functions, so these function block until the full action (taking off, go forward 1 meter, landing) is fully executed.

So technically all is fine to try out your drone, however you have to make sure that it also sends the external position at the same time. I have a simple example of the regular HL commander here which only takes off, hovers a bit and lands:

Code: Select all

import sys
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/E7E7E7E7E7')


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(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 activate_high_level_commander(cf):
    cf.param.set_value('commander.enHighLevel', '1')

def run_sequence(cf):
    commander = cf.high_level_commander

    commander.takeoff(1.0, 2.0)
    time.sleep(6.0)
    commander.land(0.0, 2.0)
    time.sleep(2)
    commander.stop()

if __name__ == '__main__':
    cflib.crtp.init_drivers()

    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf
        activate_high_level_commander(cf)
        reset_estimator(cf)
        run_sequence(cf)
What you have to add is to implement the external position send to the crazyflie.
Post Reply