Inconsistent Flight Results

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
youngbin1130
Member
Posts: 42
Joined: Thu Dec 12, 2019 4:28 am

Inconsistent Flight Results

Post by youngbin1130 » Sat Jul 11, 2020 4:18 pm

Hi again,

I have another problem and I can't seem to wrap my head around what's causing these issues.

I'm trying to fly three sets of drones. One of these drones will float in place (around 0.4m above the ground) while the other two drones will find its way to the stationary drone and go around it. This action is given through an external learning model.

However, every attempt at running the code results in a different behavior. I've attached a Google Drive link to compilation of some videos of our tests here: https://drive.google.com/drive/folders/ ... sp=sharing

As you can see some of the videos, the drones sometimes just flip at take off or randomly drop in the middle of the process (while the propellers are turning) or just rush towards one direction.

Some of the problems I think might be related to it:
1. The LPS nodes. When I connect to a crazyflie using the client, the LPS nodes are not consistently turned on (whiches from red to green to red etc)
2. The Flow deck + LPS combination. This is a known issue at https://github.com/bitcraze/crazyflie-f ... issues/368 but I'm guessing this could be the reason why the drones are not floating (because of the kalman filter).
3. The drone itself. Maybe after multiple crashes it has been damaged but there is no way for me to know.

I've attached my code here just incase you may need some reference.

Code: Select all

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017-2018 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.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
    x2      x1      x0
y3  10              4
            ^ Y
            |
y2  9       6       3
            |
            +------> X
y1  8       5       2
y0  7               1
"""
import time
import math
#from vectors import Point, Vector
import vectormath as vmath
import numpy as np
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
# Bring in CSV
import csv


####################### start of changes by darrence ##################################
import importlib
mod = importlib.import_module('DSOInferenceModule-v1')
no_of_agents = 3
path_pb = "D:\DSO-AISwarm\crazyflie-lib-python\examples\swarm\DSOfrozen_graph_def.pb"
infer = mod.InferenceModule(path_pb)
observations = []
furtherest_allowable_distance = 0.7  #this is dm in slide 7
####################### end of changes ##################################
# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/78/2M/E7E7E7E7E7'
URI3 = 'radio://0/80/2M'

URI4 = 'radio://0/78/2M'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'

# URI1 is the TARGET

uris = {
    URI1,
    URI2,
    URI3,
#    URI4,
#    URI5,
#    URI6,
#    URI7,
#    URI8,
#    URI9,
#    URI10
}


location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation
g_distance = {} # Nested Dicationary of Drone A to Drone B~X
g_bearing = {} #Nested Dictionary of Drone A to Drone B~X

            # [dist_tgt, bearing_tgt, dist_agt, bearing_agt, orient_agt]

inputLM =   [0,0,0,0,0]

for x in uris:
    g_distance[x]={}

for x in uris:
    g_bearing[x] = {}

def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Stabilizer', period_in_ms=10)
    log_config.add_variable('stabilizer.roll', 'float')
    log_config.add_variable('stabilizer.pitch', 'float')
    log_config.add_variable('stabilizer.yaw', '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:
        endTime = time.time() + 10

        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 wait_for_param_download(scf):
    while not scf.cf.param.is_updated:
        time.sleep(1.0)
    print('Parameters downloaded for', scf.cf.link_uri)


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(scf,uri, timestamp, data, logconf):
    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']

    # print("Uri: {}, Data: {}".format(uri, data))

    point = (x,y,z) # (-39.528446197509766, -8.916569709777832, -0.5112752318382263)\


    location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)
    #rotation[uri] =

    for k,v in location.items():

        # Calculate distance for all drones
        _d = calculate_distance(point,v)
        g_distance[uri][k]= _d
        g_distance[k][uri]= _d

    FormLMArray(uri,g_distance,scf)

    #print(location[uri])
    #CalculateBearing(location[URI2],location[URI3])
    #return point

def FormLMArray(uri,d_dict,scf):

    cf = scf.cf


    # Form list of keys and values
    key_list = list(d_dict[uri].keys())
    val_list = list(d_dict[uri].values())

    # Find Distance to target
    dist_tgt = d_dict[uri]['radio://0/40/1M']

    # Find Bearing to Target
    bearing_tgt = CalculateBearing(uri,location[uri], location[URI1])

    # find shortest agents and its URI
    dist_agt = min(filter(lambda x: x>0.0, val_list)) # Distance to nearest agent
    dist_agt_uri = key_list[val_list.index(dist_agt)]  # nearest agent URI

    # Find bearing for shortest agent

####################### start of changes by Darrence ##################################
    bearing_agt = CalculateBearing(uri,location[uri],location[dist_agt_uri])
    orient_agt = CalculateBearing(dist_agt_uri,location[dist_agt_uri],location[uri]) # Orientation of the nearest neighboring agent
####################### end of changes ##################################

    inputLM[0] = distance_tgt_transformation(dist_tgt)  # Distance to the target
    inputLM[1] = bearing_tgt                            # Bearing to the target
    inputLM[2] = distance_agt_transformation(dist_agt)  # Distance to the neareast neighboring agent
    inputLM[3] = bearing_agt                            # Bearing to the neearest neighboring agent
    # inputLM[4] = rotation[dist_agt_uri][2] #get YAW value
    inputLM[4] = orient_agt                             # Orientation of the neareast neighboring agent
####################### start of changes by Darrence ##################################

    observations.extend(inputLM)
    ob = []
    ob.append(inputLM)
    actions = infer.inferenceMod(ob)

####################### end of changes ##################################

    cf.commander.send_hover_setpoint(0,0,0,0.4)

    if cf.link_uri == URI1:
        pass

    else:

        #print(val_list)
        print(uri, actions)

        velocity_v = float(actions[0][0])*0.2
        rotation_r = float(actions[0][1])*35
        scf.cf.commander.send_hover_setpoint(vx=velocity_v, vy=0,yawrate=rotation_r, zdistance=0.4)

    return(inputLM)

####################### start of changes by Darrence ##################################

def distance_tgt_transformation(act_distance):
    dm=0.35 #0.35
    dt=0.07
    Dt=0.01 #0.25~0.5
    return(min(dt*(act_distance/Dt),dm))

def distance_agt_transformation(act_distance):
    dm=0.35 #0.35
    dt=0.07
    Dt=0.25 #0.25~0.5
    return(min(dt*(act_distance/Dt),dt))
####################### end of changes ##################################

def stablizer_callback(uri, timestamp, data, logconf):
    roll = data['stabilizer.roll']
    pitch = data['stabilizer.pitch']
    yaw = data['stabilizer.yaw']

    #print("Uri: {}, Data: {}".format(uri, data))

    stabilizer = (roll, pitch, yaw)

    rotation[uri] = stabilizer
    return(stabilizer)

    #CalculateStabDiff(rotation[URI2],rotation[URI3])

    # Get Position Data
def GetPosition(scf):

    log_conf = LogConfig(name='Position', period_in_ms=10)
    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(lambda timestamp, data, logconf: position_callback(scf,scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()

    # Get Roll, Pitch, Yaw
def GetStabilizer(scf):
    log_conf = LogConfig(name='Stablizer',period_in_ms=10)
    log_conf.add_variable('stabilizer.roll', 'float')
    log_conf.add_variable('stabilizer.pitch', 'float')
    log_conf.add_variable('stabilizer.yaw', 'float')
    scf.cf.log.add_config(log_conf)

    log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: stablizer_callback(scf.cf.link_uri, timestamp, data, logconf))
    log_conf.start()


    # Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
    diff = [0,0,0]
    diff[0] = p1[0]-p2[0]
    diff[1] = p1[1]-p2[1]
    diff[2] = p1[2]-p2[2]

    result = (math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2])))
    return(result)


####################### start of changes by darrence ##################################
def CalculateBearing(uri, p1,p2):

    # Finding Forward Vector of D1
    #p1_l = location[p1]
    #p2_l = location[p2]
    #yaw is rotation along z-axis but measured using x-axis
    nVector1 = vmath.Vector2(0,0)
    yawinrad = np.radians(rotation[uri][2])
    nVector1[0] = math.cos(yawinrad)
    nVector1[1] = math.sin(yawinrad)

    # Finding Directional Vector from D1 to D2
    #Vector2 = vmath.Vector2(0,0)
    #Vector2[0] = p2[0] - p1[0]
    #Vector2[1] = p2[1] - p1[1]

    #angle = nVector1.angle(Vector2,unit='deg')
    #signed_angle = np.arctan2(Vector2[0],Vector2[1]) - np.arctan2(nVector1[0],nnVector1[1])
    p1= np.asarray(p1)
    p2= np.asarray(p2)
    #p3 = np.asarray(p3)
    v1 = nVector1
    v2 = p2-p1
    bearing =-1*np.rad2deg(math.atan2(v1[0]*v2[1]-v1[1]*v2[0],v1[0]*v2[0]+v1[1]*v2[1]))
    return (bearing/180)
    #return(np.degrees(signed_angle)/180)
####################### end of changes ##################################


####################### start of changes by darrence ##################################
def run_sequence(scf):

    cf = scf.cf

    #Take off
    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
        time.sleep(0.1)

    #velocity_v = float(actions[0])*0.3
    #rotation = float(actions[1])*75

    #scf.cf.commander.send_hover_setpoint(vx=velocity_v,vy=0,yawrate=rotation, zdistance=0.4)
    #observations.pop()

####################### end of changes ##################################

if __name__ == '__main__':
    #logging.basicConfig(level=logging.DEBUG)
    factory = CachedCfFactory(rw_cache='./cache')
    cflib.crtp.init_drivers(enable_debug_driver=False)

    x = 5
    clock = -x

    with Swarm(uris,factory=factory) as swarm:

        swarm.parallel_safe(wait_for_param_download)
        swarm.parallel(reset_estimator)
        #swarm.parallel(run_sequence)

        while True:
            if time.perf_counter() >= clock + x:
                print('Waiting for parameters to be downloaded...')

            ####################### start of changes by darrence ##################################
                swarm.parallel_safe(GetPosition)
                swarm.parallel_safe(GetStabilizer) #brough this line above of swarm.parallel_safe(GetPosition) because rotation{} needs to be populated first

                #swarm.parallel_safe(calculate_distance)
                #swarm.parallel(CalculateBearing)
            ####################### end of changes ##################################


If you take a look at the function

Code: Select all

def FormArrayML:
I make all of the drones take off using

Code: Select all

    cf.commander.send_hover_setpoint(0,0,0,0.4)
and depending on the URI, it will stay in place or find its way to the stationary drone.



kimberly
Bitcraze
Posts: 456
Joined: Fri Jul 06, 2018 11:13 am

Re: Inconsistent Flight Results

Post by kimberly » Mon Jul 13, 2020 9:46 am

Hi!

I don't have access to those videos in the google drive yet so I will wait until I have.

1- With the LPS nodes, red means that the LPS node is not on the same method as your crazyflie. Try to go to this process again and press the change method button multiple times. If this does not work at all you might need to reconfigure the ones that are not switching properlly with an micro usb cable.
2- I tried this out myself last week with the LPS deck and flowdeck, and this hanging issue is only a problem with TWR. Also with the flowdeck, it is important to have enough texture on the ground and sufficient lighting or else the positioning will become unstable, even with the LPS node.
3- A quick test to see how battle scarred your drones are, is the propeller test in the console tab.

You mentioned that one of your drones flips and the propellers are still turning. What is the firmware version of your crazyflie? It seems then you are working with an older version where the flip detection hasn't been implemented yet, and I have noticed that there is an incompatibility with the LPS system and version 2019.09.

Post Reply