Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

Module2 - proj - Project phase 2

This examination task is structured as a project in which you will learn more about PID control by using it
to fly the Crazyflie aided by the Loco Positioning System (LPS). Before we dive into the material, we will
recapitulate some of the previous modules in a getting started section.

For this purpose, we assume that you have followed the previous guides on how to flash the Crazyflie,
flash the LPS Nodes and set up the LPS system. We also assume that you have a working environment
from Module2 - proj - Project phase 1 (https://kth.instructure.com/courses/4962/pages/module2-proj-
project-phase-1) to change the firmware on the Crazyflie if you want and to run the Crazyflie client.

In this part of the project we will write a python program to control the drone.

If you want to use python3 (recommended) make sure that you have pip3 (and obviously python3)
installed first. On you WASP VM you would do (to also be able to run cfclient should you want)

sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg

To install the code that you start from and the crazyflie python library do

mkdir -p ~/module2/
cd ~/module2
git clone https://github.com/pjensfelt/wasp_as1_project.git
cd ~/module2/wasp_as1_project
virtualenv -p python3 cfenv
source cfenv/bin/activate
pip install -r requirements.txt

If you prefer to use python2 change the argument for the command virtualenv to python2. Remember
(from Module1 - Mtrl - Computer vision (https://kth.instructure.com/courses/4962/pages/module1-mtrl-
computer-vision) ) that you leave the virtualenv by executing deactivate and that you need to activate the
virtualenv later as well if you want to use it.

Ensure that you have a working installation by

python cf_pc_control.py

If you have the Crazyflie radio connected you should get something like this

Scanning for Crazyflies...

Found Crazyflies:
- radio://0/108/2M
- usb://0
Connecting to radio://0/108/2M
Connected to radio://0/108/2M
Waiting for position estimate to be good enough...
Initial positional reference: [0. 0. 1.]
Initial thrust reference: 0
https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 1 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

Initial thrust reference: 0

Ready to go. Press e to enable motors
ref: (0.0, 0.0, 1.0, 0.0)
pos: (0.0, 0.0, 0.0, 0.0)
vel: (0.0, 0.0, 0.0)
error: (0.0, 0.0, 1.0)
control: (0.0, 0.0, 0.0, 0)

If you do not have the radio connected or the Crazyflie on you would get

Scanning for Crazyflies...

No Crazyflies found!

We will get back to using it later. For now, close the program by pressing 'Q'. If you do not find any
Crazyflie ensure that you have plugged in the radio. If you use the WASP VM and the Crazyflie radio is
not being recognised properly by your system you probably need to do the following on the command
line (extracted from Module2 - own (https://kth.instructure.com/courses/4962/pages/module2-own) ).

# Set up permissions
sudo groupadd plugdev
sudo usermod -a -G plugdev $USER
echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="1915", ATTRS{idProduct}=="7777", MODE="0664", GROUP="plugd
ev"' | sudo tee /etc/udev/rules.d/99-crazyradio.rules
echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE="0664", GROUP="plugd
ev"' | sudo tee /etc/udev/rules.d/99-crazyflie.rules
sudo udevadm control --reload-rules
sudo service udev restart

If plugged in, unplug the radio dongle and plug back in.

Mac

The instructions for setting things up should work in exactly the same way assuming python in installed

Windows

Should be the same with small modifications.

1. Using the Local Posi!oning System (LPS)

This is a short recapitulation of the last steps of the LPS setup guide
(https://kth.instructure.com/courses/4962/pages/setting-up-the-loco-positioning-system) . The anchors should
be placed in a room so as to maximize the volume they span. Having placed the anchors, determine
their positions in a global frame of reference, for instance by setting the origin of the global coordinate
system at the position of anchor 0, and measuring all other anchor positions relative this frame using a
laser pointer. Assume the floor is z=0 (and use a right-handed coordinate system). This measurement
step only needs to be done once assuming the anchors are placed (or re-placed) at fixed positions. For

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 2 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

instance, in the Lund robotics lab, we have the positions :

ID:0, x: 0.00, y: 0.00, z: 0.00

ID:1, x: 0.00, y: 4.50, z: 0.00
ID:2, x: 2.55, y: 0.00, z: 2.14
ID:3, x: 2.55, y: 4.50, z: 2.14
ID:4, x: 0.00, y: 0.00, z: 3.05
ID:5, x: 0.00, y: 4.50, z: 3.05

To enter this data, launch the cfclient (https://github.com/bitcraze/crazyflie-clients-python) and add the
“Loco Positioning” tab. Switch on your Crazyflie and put on the UWB tag also found in the positioning
package. Connect to the Crazyflie via the client and switch to an arbitrary address with 2Mbit/s speed.
Restart the Crazyflie for the changes to take effect.

Power on the Crazyflie again, connect with it using the client and set the anchor positions in the “Loco
Positioning” tab. Press the “write” button to configure the anchors. To verify that the anchors have been
configured correctly, disconnect from the Crazyflie, restart the GUI and reconnect to the Crazyflie. Press
the “read” button of the “Loco Positioning” tab. The addresses you wrote to the anchors should once
again be visible in the client and when moving the Crazyflie, you should see the blue marker move
around in the plots when physically displacing the UAV. Be careful not to “write” initially when all the
anchor position fields are set to zero, as this will over-write the positions stored in the anchors -
a modest inconvenience. The setup in Lund is shown in the figure below:

2. An Introduc!on to the Project Phase 2

You should now have set up the LPS system correctly, and simply starting the Crazyflie with the
positioning deck (sometimes called "tag") on will initialize the so called extended Kalman filter (EKF) run
onboard the UAV. This enables the logging of positional estimates. One of a few ways that this might
fail is if you put the deck on in reverse - it should be placed on top of the Crazyflie, with the

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 3 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

forward direction on the tag matching the forward direction of the Crazyflie (marked with small
arrows on the hardware).

We will now abstract some of the rather complex control loops to the computer, enabling positioning with
the LPS through a cascaded PD controller structure such that you may operate the Crazyflie from the
terminal using your computer keyboard. The assignment will be done in a set of steps, leaving you with
quite a bit of freedom in the software design. The objective is to compute a set of control signals on your
computer using the positional information provided by the LPS, and control the Crazyflie using the
cflib (https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib) module in the crazyflie-lib-
python (https://github.com/bitcraze/crazyflie-lib-python) stack. The aim is to implement

A functional and nicely tuned PD or PID controller - enabling control of the Crazyflie to fly to a
manually set reference point anywhere in the flyable space.

3. Transla!onal UAV Control in Theory

To clarify the assignment, it will be helpful to consider the control architecture of the Crazyflie firmware
you have been using up until this point in a bit more detail. To this end, we let (𝜙(𝑡), 𝜃(𝑡), 𝜓(𝑡) ∈ ℝ3 [rad]
denote the attitudes (pitch, roll and yaw) of the UAV as intrinsic ZYX-Euler angles
(https://en.wikipedia.org/wiki/Euler_angles) . Furthermore, we let (𝑥(𝑡), 𝑦(𝑡), 𝑧(𝑡) ∈ ℝ3 [m] denote the

translation of the UAV in the global frame of reference. By convention, we denote the time-derivative of a
⎛⎞ ˙⎛ ⎞
signal 𝑋(𝑡) as 𝑑𝑋⎝⎜𝑡⎠⎟/𝑑𝑡 ≜ 𝑋⎝⎜𝑡⎠⎟ (the dot over X is quite hard to see!). In this discussion, we refer to
variables sub-indexed by ( ) as references, and variables sub-indexed with a hat ( ^ ) as an estimated
variable. Let 𝑒𝑋 (𝑡) denote the error-state corresponding to a variable 𝑋. An error between the intended 𝑦-
velocity and the estimated 𝑦-velocity is thus defined by
⎛ ⎞ ˙ ⎛ ⎞ ˙⎛ ⎞
𝑒𝑦˙ ⎜𝑡⎟ ≜ 𝑦𝑟 ⎜𝑡⎟ − 𝑦^ ⎜𝑡⎟.
⎝⎠ ⎝⎠ ⎝⎠
Finally, 𝑇(𝑡) ∈ [0, 65635] is a unitless thrust, i.e., the sum of all forces generated by the rotors along the
body 𝐳-axis converted into a 16-bit integer, with 65535 being the maximum possible thrust the motors are
capable of generating and 0 corresponding to the rotors generating no thrust (and the motors not
https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 4 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

turning).

Controlling the UAV in “open-loop” - i.e., manually setting the PWM signals, for example, by mapping
them to the stick on a gamepad, makes any manual flight virtually impossible. When controlling the UAV
using your phone or a gamepad without any positioning system, you are instead helped greatly by a set
of stabilizing controllers run in the Crazyflie firmware (onboard the UAV). In this standard mode of
control, you will manually send a signal defined by
𝐮(𝑡) = (𝜙𝑟 (𝑡), 𝜃𝑟 (𝑡), 𝜓˙ (𝑡), 𝑇𝑟 (𝑡)) ∈ ℝ4
to the UAV. On your phone your left finger is controlling roll/pitch and you right finger is controlling
way/thrust. Roll/pitch allows us to control the motion in the plane, yaw to rotate and the overall thrust to
control the height. We have a rotational state-estimator running on the UAV (used in the first assignment)
^ ^ ^ (𝑡) ∈ ℝ3 , which allows for good estimates of the pitch and roll angles. The yaw-estimate is
(𝜙(𝑡), 𝜃(𝑡), 𝜓
drifting as you will have seen in Module2 - proj - Project phase 1
(https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-1) if you placed the UAV still on
the desk and observed the yaw-estimate in the plotter tab in the Crazyflie client. Consequently, one can
make a internal PD-controller to maintain a desired pitch and roll on the UAV. Since we directly measure
the yaw rate with the gyro a simple P control can be used to keep the yaw-rate at a desired value. The
resulting torques (the ones that ensure that the UAV gets the desired are roll and pitch angles and yaw-
rates) are combined with the thrust and are converted into motor PWM-signals using maps similar to
those identified in the system identification-part of the Crazyflie experiments, and the control system is
shown in Figure 2. The Black box is onboard the Crazyflie written in C (code is provided). Your task in
this assignments is to calculate appropriate reference signals corresponding to the green arrows to make
the UAV fly to set-points. The signals will be calculated on your computer and sent to the UAV via
bluetooth radio. You will write this code in Python.

If you tried to fly the Crazyflie with your, you will likely have found the Crazyflie flyable, but difficult to
control. By introducing the LPS, we can add another layer in the controller structure, and map errors in
translation to suitable reference angles in pitch, roll, a reasonable yaw-rate and a thrust. The idea is to
close the partially open loop by a feedback of positional estimates from the LPS. This idea is illustrated
graphically in the figure below. Your task is thus to write the code in the green boxes.

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 5 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

How to construct the translational controller? A very simple approach could be to assume that the UAV
will be close to a hovering state and that the yaw is zero, , at all times. With these assumptions,
let 𝑚 ≈ 0.027m [kg] and 𝑔 ≈ 9.81g [ ]. Simply take five controller parameters 𝐾𝑧𝑝 , ​ 𝐾𝑧𝑑 ​, 𝐾𝑝𝑝 ​, 𝐾𝑝𝑑 ​,
𝜓
𝐾𝑑 > 0​, with a constant 𝐶 > 0 [1/N] mapping the thrust required to hover, 𝑚𝑔, to a corresponding control
signal thrust. Let

where the roll has been negated, as the legacy “attitude” defined in the Crazyflie firmware corresponds
to intrinsic ZYX-Euler angles (https://en.wikipedia.org/wiki/Euler_angles) with an inverted roll…!

Question: Based on your previous experiments identifying the thrust to PWM map - what, roughly
should the constant 𝐶 be?

Slightly more difficult question: Based on the previous simulation experiments on height control, can
https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 6 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

Slightly more difficult question: Based on the previous simulation experiments on height control, can
you guess reasonable values on the and -parameters?

Even more difficult question: Some thoughts reveal that the translational dynamics, i.e. relationship
between pitch/roll and (x,y) is also double integrators. A small pitch angle of [radians] will give an
acceleration of in the x-direction. Can you from this guess reasonable values on and ?

As long as yaw is zero we know that pitch maps to motion in x and roll to motion in y. However, when the
yaw is nonzero, we need to translate the UAVs pitch/roll directions to the (x,y) coordinate system.

This can be done by changing control equations to

⎧ 𝐶
⎪
𝑧
𝑇𝑟 (𝑡) = 𝑒 𝑡 + 𝐾𝑧𝑑 𝑒𝑧˙ (𝑡) + 𝑚𝑔)
cos(𝜙)cos(𝜃)(𝐾𝑝 𝑧 ( )
⎪
⎪ ⎡ 𝜙 (𝑡) ⎤ ⎡ ⎡ ⎤
^ ) ⎤⎢ + (𝐾𝑝 𝑒𝑥 (𝑡) + 𝐾𝑑 𝑒𝑥˙ (𝑡))
𝑝
⎥
𝑝
⎪ ⎢ ⎥ ⎢ ⎥
^ ) − sin (𝜓
cos (𝜓
⎨ 𝑟 = ⎢ ⎥​
⎪ ⎢⎣ 𝜃𝑟 (𝑡) ⎥⎦ ⎢⎣ sin (𝜓 ^) ⎥ ⎦⎢⎣ − (𝐾𝑝 𝑒𝑦 (𝑡) + 𝐾𝑑 𝑒𝑦˙ (𝑡))
𝑝 𝑝
⎥⎦
^ ) cos (𝜓
⎪
⎪ 𝜓˙ 𝑟 (𝑡) = 𝐾𝜓𝑑 𝑒𝜓˙ (𝑡)
⎪
⎩

These are merely two suggestions on how to implement an ad-hoc PD controller. You are advised to
start simple and experiment with the controllers above, before possibly trying something more advanced.
For those of you with some control background, experiments with integral action, controller discretization,
saturation functions, and other forms of control improvements are encouraged!

Note that full positional control along the lines of the controllers above has already been implemented
and tuned for the Crazyflie, and can be enabled in the onboard firmware. However, for the purposes of
this project, we will implement a similar controller in Python on your laptop computer using a
nice API instead. To this end, we will begin by studying a few example implementations.

4. Studying some Python Examples

In order for you to implement the Python controllers on your laptop computer, we will start by studying a
set of examples in crazyflie-lib-python/examples/ (https://github.com/bitcraze/crazyflie-lib-
python/tree/master/examples) (you find them in ~/module2/project/src/cflib/examples if you installed as
described above).

Three questions are of critical importance when implementing the controller:

1. How do we establish a radio connection to the Crazyflie?

2. How do we send the control signal 𝐮(𝑡) to the Crazyflie?
⎛ ˙ ˙ ˙
3. How do we log the estimated state ⎜𝑥^ , 𝑦^ , 𝑧^ , 𝑥^ , 𝑦^ , 𝑧^ , 𝜙^ , 𝜃^ , 𝜓
^
⎝

Once we have answered these questions, implementing the controller will hopefully be easy!

4.1. Establishing a Connec!on

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 7 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

4.1. Establishing a Connec!on

The cflib (https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib) module offers some nice

functionality for “scanning” for Crazyflies and “connecting” to any found Crazyflie, similar to what we do in
the cfclient. An illustrative and minimal example of this is found in scan.py

(https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/scan.py) in the cflib

(https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib) module.

4.2. Wri!ng a Control Signal

Having established a connection with the Crazyflie, we may then send commands to it. The script
ramp.py (https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/ramp.py) shows how to
define a cflib.crazyflie (https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib/crazyflie) -
object using Bitcraze’s API, and subsequently send control signals to it using the function

crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)

The example ramps up the rotor speeds and shows how you may communicate your computed control
signals to the Crazyflie.

4.3. Logging an Es!mate

The cflib (https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib) module is callback-driven,

and the script basiclog.py (https://github.com/bitcraze/crazyflie-lib-
python/blob/master/examples/basiclog.py) shows how to set up the logging of parameters from the
Crazyflie using cflib.crazyflie.log (https://github.com/bitcraze/crazyflie-lib-
python/blob/master/cflib/crazyflie/log.py) by defining log blocks, similar to what we have done in the client.
To the log block, we may attach a callback which stores the data such that it may be accessed by the
control loop.

HINT: To see which log-blocks are defined on the Crazyflie, you may either use the cflib

(https://github.com/bitcraze/crazyflie-lib-python/tree/master/cflib) API, or check the log table of contents

using the Crazyflie Client GUI. For instance, the estimated x-translation may be logged from the log-
block “kalman” with a logging parameter stateX.

5. - Tour of the code you start from

To get you started quickly and with a focus on the core part of this project we have provided you with a
functional code skeleton cf_pc_control.py
(https://github.com/pjensfelt/wasp_as1_project/blob/master/cf_pc_control.py) that can

Connect to the Crazyflie (either at a fixed URI or scan and connect to the first found)
Get position and attitude data from the UAV using the logging mechanism described above
Converts the quaternions attitude representation to a rotation matrix and Euler angles. For this we
make use of a custom version of the ROS/tf functionality
from https://github.com/davheld/tf/blob/master/src/tf/transformations.py
https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 8 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

from https://github.com/davheld/tf/blob/master/src/tf/transformations.py
(https://github.com/davheld/tf/blob/master/src/tf/transformations.py) .
Reads keys being pressed on the keyboard to control what happens (change ref pos, enable/disable
motors, etc)
Sends control reference signals for roll, pitch, yaw rate and thrust to the inner control loops on the
UAV
Printouts out the position, velocity, error and control signals on the screen for debugging
Logs state estimates from the onboard Kalman filter and your control signals to file for offline analysis
By pressing
'e' and 'q' you can enable and disable the motors
'>' and '<' you can manually increase and decrease the thrust
'Q' to quit

Prep task 1:

First study the code so that you understand its function. Note that your controller code eventually will go
into the function calc_control_signals()

Prep task 2:

Test the code in a safe setting. You do not need the LPS system for this test. To run you do

cd ~/module2/wasp_as1_project
source cfenv/bin/activate
python cf_pc_control.py

Prep task 3:

Move the drone around in the space and study the position information you get from the LPS to get a
feeling for how it varies.

6 Transla!onal UAV Control in Prac!ce

Your next task is to implement a controller which maps the errors

(𝑒𝑥 (𝑡), 𝑒𝑦 (𝑡), 𝑒𝑧 (𝑡), 𝑒𝑥˙ (𝑡), 𝑒𝑦˙ (𝑡), 𝑒𝑧˙ (𝑡), 𝑒𝜓 (𝑡)) ∈ ℝ7
to a control signal on the form
˙
𝐮(𝑡) = (𝜙𝑟 (𝑡), 𝜃𝑟 (𝑡), 𝜓𝑟 (𝑡), 𝑇𝑟 (𝑡)) ∈ ℝ4

Look at the suggestions in Section 3.

When tuning the controller, it is highly advised to hang the UAV in a piece of string such that is doesn’t
crash if you set parameters yielding an explosively unstable system response. See the figure below for
inspiration of how to create such a setup with some string and a straw (to decrease the risk of hitting the
string with the rotors).

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 9 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 10 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

Main Task:
Implement a controller of your choice, for example a variant of the previously mentioned controllers. Tune
the controller to your satisfaction (it must be capable of sustained flight).

Create an application such that the drone can start from one A4 paper size region (you may need to
make it bigger if your position information is not good enough), take off and land in another region and
then fly back a forth a few times.

Extra Assignment: Impress us !

Documenta!on:
Get two videos of the UAV flying in the LPS system, corresponding to the main task above and submit
https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 11 of 12
Module2 - proj - Project phase 2: WASP Autonomous Systems 1 VT18 2018-05-08, 15)54

Get two videos of the UAV flying in the LPS system, corresponding to the main task above and submit
your code in a file my_controller.py along with the video on Canvas. Make sure to include docstrings in
the code so that it is human-readable!

A video of the controller is given in the link below, and you should be able to generate similar results with
the above instructions for the task.

WASP - Positional PID-control using the Python API (https://www.youtube.com/watch?

v=G0XBegcuSvo)

(https://www.youtube.com/watch?v=G0XBegcuSvo)

Authored by Marcus Greiff on behalf of WASP

https://kth.instructure.com/courses/4962/pages/module2-proj-project-phase-2 Page 12 of 12