Vicon Control of CF 2.0

[nrg0004]

I am trying to control the Crazy Flie using Vicon camera system. I have markers set on the CF to read its X,Y,Z and yaw angles. I am then using a PID position control loop to feedback new values of pitch, roll, yawrate and thrust pwm to the CF to either maintain its position or follow a trajectory. When I apply the feedback, the CF does not perform as defined in the trajectory. For Ex., if my feedback gives a positive roll angle, the CF is logging a negative roll angle. I am assuming that I maybe wrong at few places and/or the physical properties of the CF are changing. Note that I use both python and MATLAB to perform this. I use MATLAB to get data from the Vicon and perform the calculations and send data to python. There is currently a maximum delay of 0.03 s between every readings. The code is attached below:

Code: Select all

# -*- coding: utf-8 -*-


import sys
sys.path.append("../lib")

import cflib.crtp

import logging
import time
import datetime
from threading import Timer
import math
import matlab.engine
eng = matlab.engine.start_matlab()
import cflib.crtp
from cfclient.utils.logconfigreader import LogConfig
from cflib.crazyflie import Crazyflie

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)

file1 = open("IMU.txt","w")
file2 = open("Vicon.txt","w")
file3 = open("U vector.txt","w")
##file4 = open("Vicon_Position.txt","w")

class LoggingExample:
    """
    Simple logging example class that logs the Stabilizer from a supplied
    link uri and disconnects after 5s.
    """
    def __init__(self, link_uri):
        """ Initialize and run the example with the specified link_uri """

        # Create a Crazyflie object without specifying any cache dirs
        self._cf = Crazyflie()
        

        # Connect some callbacks from the Crazyflie API
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

        print "Connecting to %s" % link_uri

        # Try to connect to the Crazyflie
        self._cf.open_link(link_uri)

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

    def _connected(self, link_uri):
        """ This callback is called form the Crazyflie API when a Crazyflie
        has been connected and the TOCs have been downloaded."""
        print "Connected to %s" % link_uri
##        Thread(target=self._ramp_motors).start()

        ####

                    
## Adding a separate log config for accelerometer data
            
        self._lg_stab1 = LogConfig(name="Stabilizer", period_in_ms=20)
        self._lg_stab1.add_variable("gyro.x", "float")
        self._lg_stab1.add_variable("gyro.y", "float")
        self._lg_stab1.add_variable("gyro.z", "float")
        

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_stab1)
            # This callback will receive the data
            self._lg_stab1.data_received_cb.add_callback(self._stab1_log_data)
            le._cf.commander.send_setpoint(0, 0, 0, 0)
            # This callback will be called on errors
            self._lg_stab1.error_cb.add_callback(self._stab1_log_error)
            # Start the logging
            self._lg_stab1.start()
        except KeyError as e:
            print "Could not start log configuration," \
                  "{} not found in TOC".format(str(e))
        except AttributeError:
            print "Could not add Stabilizer log config, bad configuration."

        ####

        # The definition of the logconfig can be made before connecting
        self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=20)
        self._lg_stab.add_variable("stabilizer.roll", "float")
        self._lg_stab.add_variable("stabilizer.pitch", "float")
        self._lg_stab.add_variable("stabilizer.yaw", "float")
        self._lg_stab.add_variable("acc.x", "float")
        self._lg_stab.add_variable("acc.y", "float")
        self._lg_stab.add_variable("acc.z", "float")
        

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            le._cf.commander.send_setpoint(0, 0, 0, 0)
            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            self._lg_stab.start()
        except KeyError as e:
            print "Could not start log configuration," \
                  "{} not found in TOC".format(str(e))
        except AttributeError:
            print "Could not add Stabilizer log config, bad configuration."




        # Start a timer to disconnect in 10s
        t = Timer(15, self._cf.close_link)
        t.start()
        
    def _stab_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print "Error when logging %s: %s" % (logconf.name, msg)

    def _stab_log_data(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        
        #print "[%d][%s]: %s" % (timestamp, logconf.name, data)
        
##        acc_x = (data["acc.x"]+0.0039)* 9.807
##        acc_y = ( data["acc.y"] - 0.00063739)*9.807
##        acc_z = (data["acc.z"] - 0.9871)*9.807


        global pos_x_old, pos_y_old , pos_z_old , Time1, MS1_past,Time2, MS2_past, P_est_past, u
        global ep_int_past ,ea_int_past
        
##        global pos_y_old 
##        global pos_z_old 

        acc_x = (data["acc.x"])* 9.871
        acc_y = ( data["acc.y"])*9.871
        acc_z = (data["acc.z"] - 0.9871)*9.871
        roll = data["stabilizer.roll"]
        pitch = data["stabilizer.pitch"]
        yaw = data["stabilizer.yaw"]
              
        time = str(datetime.datetime.now().time())
        time = time.split('.')
        MS1 = float(time[1])
        Dt = (MS1 - MS1_past)/1000000
        if Dt < 0:
            Dt = 1+ Dt
        if Time2 == 0:
            Dt = 0
        Time1 = Time1 + Dt   
        file1.write(str(pitch)  + "\t" + str(roll) + "\t" + str(yaw)+ "\t" + "\t" + str(acc_x)  + "\t" + str(acc_y) + "\t" + str(acc_z)+ "\t" + "\t"  + str(self.gyro_x)  + "\t" + str(self.gyro_y) + "\t" + str(self.gyro_z)+ "\t" + str(time[0]) + "\t" + str(time[1]) + "\t" + str(Time1)+ "\t" + str(Dt)+ "\n")

##        print "Roll is %.3f Pitch is %.3f Yaw is %.3f\n" % (roll,pitch,yaw)
##        print "Acc_x is %.5f Acc_y is %.3f Acc_z is %.3f\n" % (acc_x,acc_y,acc_z)
##        print "Roll_omega is %.3f Pitch_omega is %.3f Yaw_omega is %.3f\n" % (self.gyro_y,self.gyro_x,self.gyro_z)

        
        MS1_past = MS1

        #q_est_past = [pos_x_old,pos_y_old,pos_z_old, roll,pitch,yaw,vel_x,vel_y,vel_z,self.omega_x,self.omega_y,self.omega_z]
        
        (pos_x, pos_y, pos_z,vel_x,vel_y,vel_z,RPM,roll_des,pitch_des,yaw_des,P_est, ep_int ,ea_int ) = eng.Control_variables_test_9_2(pos_x_old,pos_y_old,pos_z_old, roll,pitch,yaw,self.gyro_x,self.gyro_y,self.gyro_z,acc_x,acc_y,acc_z,P_est_past,Time2,Dt,ep_int_past,ea_int_past,nargout=13) 
        print "X is %.3f Y is %.3f Z is %.3f\n" % (pos_x,pos_y,pos_z)
##        print "V_X is %.3f V_Y is %.3f V_Z is %.3f\n" % (vel_x,vel_y,vel_z)
        print RPM,roll_des,pitch_des,yaw_des
        print Time2
        time = str(datetime.datetime.now().time())
        time = time.split('.')
        MS2 = float(time[1])
        Dt = (MS2 - MS1)/1000000
        if Dt < 0:
            Dt = 1+ Dt
        Time2 = Time2 + Dt
        file2.write(str(pos_x)  + "\t" + str(pos_y) + "\t" + str(pos_z) + "\t" + "\t" + str(vel_x) + "\t"+ str(vel_y) + "\t" + str(vel_z) + "\t" + "\t" + str(time[0]) + "\t" + str(time[1])+ "\t" + str(Time2)  + "\n")
        file3.write(str(RPM) + "\t" + str(roll_des) + "\t" + str(pitch_des) + "\t" + str(yaw_des) + "\t" + str(Time2) + "\n")
        
        le._cf.commander.send_setpoint(roll_des, pitch_des, yaw_des, RPM)
##        le._cf.commander.send_setpoint(0, 0, 0, 38000)
        
        pos_x_old = pos_x
        pos_y_old = pos_y
        pos_z_old = pos_z
        MS2_past = MS2
        P_est_past = P_est
        ep_int_past = ep_int
        ea_int_past = ea_int
##        Time2 = Time2 + Dt

        
       
        

    def _stab1_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print "Error when logging %s: %s" % (logconf.name, msg)

    def _stab1_log_data(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        #print "[%d][%s]: %s" % (timestamp, logconf.name, data)
        self.gyro_y = data["gyro.y"] 
        self.gyro_x = data["gyro.x"] 
        self.gyro_z = data["gyro.z"]
        
        
        #print "Roll_omega is %.3f Pitch_omega is %.3f Yaw_omega is %.3f\n" % (gyro_y,gyro_x,gyro_z)#inertial_fram


    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the speficied address)"""
        print "Connection to %s failed: %s" % (link_uri, msg)
        self.is_connected = False

    def _connection_lost(self, link_uri, msg):
        """Callback when disconnected after a connection has been made (i.e
        Crazyflie moves out of range)"""
        print "Connection to %s lost: %s" % (link_uri, msg)

    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
        print "Disconnected from %s" % link_uri
        file1.close()
        file2.close()
        file3.close()
        eng.F_Vicon_stop(nargout=0)
        self.is_connected = False


if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    # Scan for Crazyflies and use the first one found
    print "Scanning interfaces for Crazyflies..."
    available = cflib.crtp.scan_interfaces()
    print "Crazyflies found:"
    for i in available:
        print i[0]
##        eng.figure(nargout=0)
        pos_x_old = 0
        pos_y_old = 0
        pos_z_old = 0
        P_est_past = 0
        Time1 = 0
        Time2 = 0
        MS1_past = 0
        MS2_past = 0
        ep_int_past = matlab.double([0,0])
        ea_int_past = matlab.double([0,0,0,0])
        u = 0
        eng.F_ViconInitialization(nargout=0)
        le = LoggingExample(available[0][0])
    
else:
    print "No Crazyflies found, cannot run example"

    # The Crazyflie lib doesn't contain anything to keep the application alive,
    # so this is where your application should do something. In our case we
    # are just waiting until we are disconnected.
    while le.is_connected:
        time.sleep(1)

The PID position control block diagram is attached. I really need more help here. I can provide more information if needed.

[marcus]

Hi,

Maybe you need to change the sign of one of the axis in your calculations. If you place the Crazyflie infront of you with the blue LED's closest to you and the front facing away (like in in this image) the changes in axes are as follows:

• Roll is positive to the right
• Pitch is positive backwards
• Yaw is positive when rotated CCW (to the left)

Make sure that this matches your calculations.

If you are planning on using ROS make sure to check out Wolfgang's ROS driver. Another way you could work with this is to use the same setup as we have used for the Kinect (there's more info here) where you could substitute the Kinect part with readings from Vicon instead.

[nrg0004]

Marcus,

Thank you for replying. I will check by using the new orientation you mentioned. I will try some more time to perfect my solution- if that fails I shall try adapting to the other two alternate projects.

I doubt I am doing a mistake with the PID tuning. I start my PID tuning assuming that the value of RPM for hovering thrust to be 38000. I use the below thrust equation in my PID MATLAB tuning:

Code: Select all

m = 0.027;
g = 9.871;
RPM_hover = 38000;

kt = m*g/(RPM_hover^2); %Thrust_hover = m*g = Kt * rpm^2
RPM = sqrt( RPM_hover^2 + k_z*ea(1)/kt +k_zi*ea_int(1)/kt+k_zd*ea_d(1)/kt);

Either the orientation or this hover thrust RPM or both could be the reason. Am I going in the right direction by choosing 38000 as the right value.

As seen in this post https://wiki.bitcraze.io/misc:investigations:thrust to get around 27 g of thrust 57% of pwm is required. I am close to that value. However, I see that this is not RPM value and it is PWM value so should I use the quadratic equation mentioned in that post?

[whoenig]

38000 sounds reasonable to me, but it depends on your payload (especially since you added some VICON markers). I think I used ~42000. However, the I-part of your controller for thrust should take care of a (limited) amount of steady-state error here as well.

The latest firmware for CF2.0 decouples thrust from battery voltage, so technically you don't need to worry about that. In fact, it does work fine in my experiments without complicated compensation.

It sounds to me like some orientations just don't match.

[nrg0004]

All,

I am trying to change the coordinate system to left hand system from what I initially used. I have a major question. What is the order of transformation that has been adopted ?
Is the transformation matrix,

R(from body to inertial frame) = Rz(yaw) x Ry(pitch) x Rx(roll)

I mean is the order ZYX or anything else (ZYZ) ?

Thank you.

[Slaxx]

I did the same.
What i did was the following:

• Calculate necessary Forces in x,y,z direction with some controller
• Transform the forces into the yaw rotated coordinate system
• Calculate the necessary thrust and angles from the Forces with Trigonometrics
• Send the setpoints to the Crazyflie

That worked fine

[binx]

Hi,

Maybe you need to change the sign of one of the axis in your calculations. If you place the Crazyflie infront of you with the blue LED's closest to you and the front facing away (like in in this image) the changes in axes are as follows:

• Roll is positive to the right
• Pitch is positive backwards
• Yaw is positive when rotated CCW (to the left)

Make sure that this matches your calculations.

If you are planning on using ROS make sure to check out Wolfgang's ROS driver. Another way you could work with this is to use the same setup as we have used for the Kinect (there's more info here) where you could substitute the Kinect part with readings from Vicon instead.

Hi,

As you said to check out Wolfgang's ROS driver, is this driver contains all the basic parts to control the Crazyflie. I can get little information about this. Thanks a lot in advance.

Bin

[arnaud]

Hi Bin,

The Wolfgang's ROS driver contains all you need to control one or many Crazyflie, including sending setpoints, getting telemetry using the log subsystem and setting parameters in the Crazyflie.

/Arnaud