Example code autonomous Sequence not working properly

All discussions related to the Loco Positioning system
A-SH
Beginner
Posts: 6
Joined: Fri May 18, 2018 6:34 pm

Example code autonomous Sequence not working properly

Post by A-SH » Fri May 18, 2018 6:57 pm

Hi all,

We have been trying to make the crazyflie move in absolute coordinate system with the help of LPS.
When we try to run the sample code "autonomousSequence.py", the crazyflie flies away out of the flying volume in some direction.
(we just changed the sequence of points in autonomousSequence to fit our smaller flying volume)
We configured anchors as shown in the tutorials. When trying to fly the crazyflie using flow-deck only, it flies successfully.

Thanks in advance !

tobias
Bitcraze
Posts: 1733
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Example code autonomous Sequence not working properly

Post by tobias » Mon May 21, 2018 11:14 am

Quick follow up. Are you starting the CF2 in the right direction? How does the position behave in the LPS tab if you move the CF2 around?

A-SH
Beginner
Posts: 6
Joined: Fri May 18, 2018 6:34 pm

Re: Example code autonomous Sequence not working properly

Post by A-SH » Mon May 21, 2018 8:55 pm

Hi

The CF2 is placed in the correct orientation initially.
I was checking LPS tab as you suggested and I guess I figured out what the problem is. However, I don't know what is the cause of it.

So, when I place the CF2 somewhere within the flying volume, the LPS tab shows that the CF2 can communicate with all 8 anchors. However, the estimate of its location is not that accurate, the range of error is not around 10 cm but significantly larger (differs from location to location but sometimes up to 50-60 cm error).

For example, when I place the CF2 on the x-axis, I expect the y coordinate to take a value close to 0. However, the estimated value shown in LPS tab is around 50-60 cm.

In my setup the distance on the y-axis is 4.6 m, but the estimate of the CF2 position in the LPS tab shows ~0.75 m and ~4.6 m respectively when I place it on the edges of the y-axis. If I restart the CF2 and connect again, I get a different estimate of its location.

I would appreciate your thoughts on what might be causing this.

Some related information on the setup of the LPS anchors:
-I have 8 anchors placed in the corners of the room.
-The anchors are at least 2m apart, and more than 20 cm away from the ceiling, floor and the closest wall.
-The anchors are in line of sight of each other.
-I am using TWR mode.

sample
Beginner
Posts: 10
Joined: Wed May 02, 2018 1:57 pm

Re: Example code autonomous Sequence not working properly

Post by sample » Tue May 22, 2018 9:03 am

Hi,

I have the same problem. We are using 8 anchors in suggested setup. Last week we have changed the anchors positions and since then we are unable to run dwm_loc_ekf_hover.launch which was working perfectly before. Now, when we run dwm_loc_ekf_hover.launch CF flies of the floor, reaches certain altitude and then just flies away in some direction, never reaching goal.
As mentioned, we get strange readings from one of the axis. Our reference coordinate frame is placed in the middle of the room. In the center, readings are very accurate (<5cm) for both of the axes, but as we get further away from the center Y readings become less and less accurate (>20cm) while X readings remain in the expected boundaries. When we place CF on the outer edge of flying space (4x3m) Y readings become more accurate again (<10cm). We have checked the room setup and anchor positions more than few time, flashed the CF and re-cloned git repositories but nothing helped. We are running ROS Kinetic on Ubuntu 16.04 and LPS i TWR mode. Do you think this strange behavior of the CF could be related to strange readings we get from the Loco on certain locations?

tobias
Bitcraze
Posts: 1733
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Example code autonomous Sequence not working properly

Post by tobias » Tue May 22, 2018 12:18 pm

My initial thought is that something is wrong with the anchor positions and possibly mixed up the axis. Are you sure these are correct?

sample
Beginner
Posts: 10
Joined: Wed May 02, 2018 1:57 pm

Re: Example code autonomous Sequence not working properly

Post by sample » Thu May 24, 2018 11:55 am

tobias wrote:
Tue May 22, 2018 12:18 pm
My initial thought is that something is wrong with the anchor positions and possibly mixed up the axis. Are you sure these are correct?
You were right, CF wasn't facing the X axis and that's why it wasn't able to hover properly. Now everything works fine. Still, problem with LPS accuracy on certain spots remains. I believe some kind of interference might be the cause. We'll look into it and provide you with feedback. Thanks for you help.

A-SH
Beginner
Posts: 6
Joined: Fri May 18, 2018 6:34 pm

Re: Example code autonomous Sequence not working properly

Post by A-SH » Mon Jun 04, 2018 5:17 pm

My apologies for the very late reply, we accidentally broke the Micro USB socket of one of the anchors and it took us until now to have the LPS up and running again.

We double checked the positions of the anchors and orientation of each of the three axes, all seems correct.

One strange thing we noticed as well is when we fly the CF2 using only the flow deck using Motion Commander it flies pretty smoothly and accurately, the same goes for when we fly the CF2 using only the LPS using Motion Commander (or using Commander).

However, when we attach both the flow deck and the local positioning node to the CF2 and attempt to fly it using Commander the flying and movement behavior of the CF2 becomes less stable and accurate, which is weird as we expect the flow deck to improve the accuracy of estimating z-distance or at least have no negative effect.

Thanks a lot for all your help :)

arnaud
Bitcraze
Posts: 1788
Joined: Tue Feb 06, 2007 12:36 pm

Re: Example code autonomous Sequence not working properly

Post by arnaud » Mon Jun 11, 2018 11:07 am

The Z-distance part of the flow deck is pretty well handled together with the LPS, if you disable pushing the flow part in the kalman filter things should be as stable as with the LPS but with better Z behaviour. Flow can be disable with this parameter: https://github.com/bitcraze/crazyflie-f ... #L235-L237

Pushing the flow and the LPS data in the kalman filter still seems to be challenging: it can work very well for a while and then suddenly snap. I have the feeling that the info from the LPS and flow are fighting each other.

A-SH
Beginner
Posts: 6
Joined: Fri May 18, 2018 6:34 pm

Re: Example code autonomous Sequence not working properly

Post by A-SH » Wed Jun 13, 2018 6:45 pm

Great ! Thanks a lot !

albussimba
Beginner
Posts: 7
Joined: Sat Sep 01, 2018 3:31 pm

Re: Example code autonomous Sequence not working properly

Post by albussimba » Tue Sep 04, 2018 3:27 am

Hi there, other then the craft must be facing in the x direction, must the nodes also be facing in the x directions? I have having some problems running the example code too. The position that I read off the the drone is accurate, but the drone still flies to one direction.

I've edited the code such that it just flies on the spot:

Code: Select all

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

# URI to the Crazyflie to connect to
uri = 'radio://0/80/2M'

# Change the sequence according to your setup
x = 1.0
y = 1.0
z = 0.0
sequence = []


def set_sequence():
    global x, y, sequence
    fixz = 1.0
    print('set pos: ({}, {}, {})'.format(x, y, fixz))
    sequence = [(round(x, 2), round(y, 2), fixz, 0.0) for _ in range(5)]




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(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 position_callback(timestamp, data, logconf):
    global x, y, z
    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.data_received_cb.add_callback(set_sequence)
    log_conf.start()



def intial_sequence(scf):

    position = (0.0, 0.0, 0.0)
    cf = scf.cf
    cf.param.set_value('flightmode.posSet', '1')

    print('Setting position {}'.format(position))
    for i in range(50):
        time.sleep(0.1)
    set_sequence()


def run_sequence(scf, sequence):

    print(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)


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

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

        start_position_printing(scf)
        intial_sequence(scf)
        run_sequence(scf, sequence)

        # while 1:
        #     intial_sequence(scf)

Post Reply