Precision Problem with the autonomous_sequence_high_level

Discussions and questions about the Crazyflie Nano Quadcopter
anomi
Beginner
Posts: 17
Joined: Wed Jun 03, 2020 6:55 pm

Precision Problem with the autonomous_sequence_high_level

Post by anomi »

Hello,

I managed to generate my trajectory using uav_trajectories this is my trajectory:
Screenshot from 2020-06-17 14-47-00.png
I run my trajectory using autonomous_sequence_high_level.py.

the problem I have is that the drone goes up with a height of 1 meter before executing my trajectory. and also I do not have a good precision. here is my video which shows my trajectory and also the problem of precision that I have.

https://youtu.be/MFmEjLBygtI

before launching my script I changed the mode to TDoa here is a screen of my cfclient as you see I have a problem of calculation for the z axis.
trajectoir.png
I want to know where my problem comes from knowing that I have installed my LPS and I have configured my crazyfie with the latest version of firmware.
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Precision Problem with the autonomous_sequence_high_level

Post by kimberly »

Hi!

Sorry for the late reply! Summer breaks and everything :)

So what I can see is that the location of the nodes in the LPS tab, do not match the locations of the LPS nodes that I see in your video. Are you sure you went through every step of the tutorial when setting it up, in particular the Anchor position configuration part?
anomi
Beginner
Posts: 17
Joined: Wed Jun 03, 2020 6:55 pm

Re: Precision Problem with the autonomous_sequence_high_level

Post by anomi »

Hello

Thank you for your reply,yes i followed all the steps to install the lps system.

I managed to fly the crazyflie according to my trajectory but the problem i have is that i don't have a good precision the crazyflie shifts to the right and to the left during the execution of my trajectory knowing that I have set the x axis at 0.35.

And also when I launch the script the crazyflie goes up with a height of 1 m before executing my trajectory
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Precision Problem with the autonomous_sequence_high_level

Post by kimberly »

Could you please share your script content? I assume your autonomous_sequence_high_level script is not the same as the original?

Just to clarify, have you also changed the anchor configurations in the configure position gui to the one that I see in your video?

Also, are you using the Crazyflie 2.0 or 2.1?
anomi
Beginner
Posts: 17
Joined: Wed Jun 03, 2020 6:55 pm

Re: Precision Problem with the autonomous_sequence_high_level

Post by anomi »

Thank you again for your answer

I use the Crazyflie 2.0. here is my autonomous_sequence_high_level for this script i just pasted my csv file generated by uav_trajectories:

Code: Select all

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2018 Bitcraze AB
#
#  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.
"""
Simple example that connects to one crazyflie (check the address at the top
and update it to your crazyflie address) and uses the high level commander
to send setpoints and trajectory to fly a figure 8.

This example is intended to work with any positioning system (including LPS).
It aims at documenting how to set the Crazyflie in position control mode
and how to send setpoints using the high level commander.
"""
import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger

# URI to the Crazyflie to connect to
uri = 'radio://0/80/250K'

# The trajectory to fly
# See https://github.com/whoenig/uav_trajectories for a tool to generate
# trajectories

# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7
figure8 = [
[1.94407,0.35,0,0,0,0,0,0,0,0,0,0,0,0.249185,-0.210906,0.066447,-0.00752572,0,0,0,0,0.245519,-0.207575,0.065594,-0.00746944,0,0,0,0,0,0,0,0
, ],
# noqa
[1.81561,0.35,1.53354e-17,2.51224e-18,9.0972e-18,0,0,0,0,0.5,0.487702,0.00269216,-0.0313613,-0.0965227,0.046441,0.00411718,-0.00340606,0.5,0.497023,0.014375,-0.0311825,-0.136679,0.0937604,-0.0178844,0.000197844,0,0,0,0,0,0,0,0
, ],
# noqa
[1.5796,0.35,-3.61885e-18,-1.11751e-18,-1.60786e-18,0,0,0,0,1,0.0331445,-0.0377929,-0.00404071,0.0244386,-0.0141881,0.00335248,-0.000278285,1,-0.00450496,-0.0803347,0.00141505,-0.430467,0.479214,-0.194482,0.0281352,0,0,0,0,0,0,0,0
, ],
# noqa
[1.68851,0.35,4.11491e-18,3.12437e-18,-1.06481e-17,0,0,0,0,1,-0.00531817,0.0053301,-1.44305e-05,0.00343343,-0.00732799,0.00412526,-0.000743177,0.5,-0.532882,0.017878,0.0389627,-0.0940468,0.266293,-0.177903,0.0356159,0,0,0,0,0,0,0,0
, ],
# noqa

]

class Uploader:
    def __init__(self):
        self._is_done = False

    def upload(self, trajectory_mem):
        print('Uploading data')
        trajectory_mem.write_data(self._upload_done)

        while not self._is_done:
            time.sleep(0.2)

    def _upload_done(self, mem, addr):
        print('Data uploaded')
        self._is_done = True


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 activate_mellinger_controller(cf):
    cf.param.set_value('stabilizer.controller', '2')


def upload_trajectory(cf, trajectory_id, trajectory):
    trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0]

    total_duration = 0
    for row in trajectory:
        duration = row[0]
        x = Poly4D.Poly(row[1:9])
        y = Poly4D.Poly(row[9:17])
        z = Poly4D.Poly(row[17:25])
        yaw = Poly4D.Poly(row[25:33])
        trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw))
        total_duration += duration

    Uploader().upload(trajectory_mem)
    cf.high_level_commander.define_trajectory(trajectory_id, 0,
                                              len(trajectory_mem.poly4Ds))
    return total_duration


def run_sequence(cf, trajectory_id, duration):
    commander = cf.high_level_commander

    commander.takeoff(1.0, 2.0)
    time.sleep(3.0)
    relative = True
    commander.start_trajectory(trajectory_id, 1.0, relative)
    time.sleep(duration)
    commander.land(0.0, 2.0)
    time.sleep(2)
    commander.stop()


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

    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf
        trajectory_id = 1

        activate_high_level_commander(cf)
        #activate_mellinger_controller(cf)
        duration = upload_trajectory(cf, trajectory_id, figure8)
        print('The sequence is {:.1f} seconds long'.format(duration))
        reset_estimator(cf)
        run_sequence(cf, trajectory_id, duration)
For the configuration I don't know which configuration you are talking about, the only configuration i did for the anchors is this : https://www.bitcraze.io/documentation/t ... ng-system/
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Precision Problem with the autonomous_sequence_high_level

Post by kimberly »

Hi,

Sorry, I double checked the screenshot and it seems indeed that the configuration matches your video.

However, the 6 anchor setup as I see that your system is configured in is mostly recommended for TWR instead of the TDOA2 and 3 (the 8 anchor one is more accepted for TDOA). Have you tried flying in TWR mode? Do you also have the two other nodes available at your disposal?

I will try to check if I can see the same thing happening with your script this week, however we do have our anchorsystem setup here as an 8 anchor setup (so as a cube), so it might come out differently here.
anomi
Beginner
Posts: 17
Joined: Wed Jun 03, 2020 6:55 pm

Re: Precision Problem with the autonomous_sequence_high_level

Post by anomi »

Hi,

Yes i have tried flying in TWR mode but I had the same problem.

here is a video shows the flies in mode TWR: https://www.youtube.com/watch?v=eOCZovuVl8M

and here is a screen of my cfclient as you see I have a good position measurement:

Image

For the anchors I only have 6 anchors as you said 6 anchors works only with the mode TWR.
Attachments
twr.png
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Precision Problem with the autonomous_sequence_high_level

Post by kimberly »

I've analyzed both videos together. In general I do not think that there is anything wrong with your setup or how the crazyflie is flying. The way it is flying is to be expected from the LPS system (10-20 cm accuracy). So what it could be is that in the trajectory generation there is a 1 meter offset somehow. we do not maintain uav_trajectory ourselves so it is difficult for us to access what is going wrong there, and also the polynomial is a bit hard to read.

What you can try is autonomousSequence.py instead (do change the sequence so that it fits your flight area) and see if the crazyflie flies to the positions that you expect. If it does, than it is something in uav_trajectory self that you will need to ask the creators of that in their repository issue list.

You can also try the beziercurves script as described in this blog as well.
anomi
Beginner
Posts: 17
Joined: Wed Jun 03, 2020 6:55 pm

Re: Precision Problem with the autonomous_sequence_high_level

Post by anomi »

I think that the problem comes from the version of firmware because I have worked with the version of 09.2019 before, and I didn't get the problem of the offset, but when I make an update towards the version of 06.2020 I get the offset problem.

here is a video showing my script with the firmware version of 09.2019: https://www.youtube.com/watch?v=2LTPqe8 ... e=youtu.be
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Precision Problem with the autonomous_sequence_high_level

Post by kimberly »

Oh wow, really? I don't see much difference with the precision but indeed the initial position seems to be way off. Its interesting to see that this is different in different releases, since we did not change that much to the LPS system.

Alright, we are going to investigate this this week! If we see the same problem here, we will make an issue out of it on the github repo. We will give an update soon.
Post Reply