diff --git a/data/Visual inspection on MAVs.odt b/data/Visual inspection on MAVs.odt
new file mode 100644
index 0000000000000000000000000000000000000000..318968c570000da2d1df75f43a17c055503f52cf
Binary files /dev/null and b/data/Visual inspection on MAVs.odt differ
diff --git a/data/kinect_lionel.cal b/data/kinect_lionel.cal
new file mode 100644
index 0000000000000000000000000000000000000000..432c700919205ab5dcd9e5be855272b3898cb628
--- /dev/null
+++ b/data/kinect_lionel.cal
@@ -0,0 +1,35 @@
+[rgb]
+principal_point\x=313.67245
+principal_point\y=264.06175
+focal_length\x=527.44654
+focal_length\y=527.40652
+distortion\k1=0.20780
+distortion\k2=-0.34320
+distortion\k3=0.00139
+distortion\k4=0.00061
+distortion\k5=0.00000
+
+[depth]
+principal_point\x=313.23400
+principal_point\y=246.13447
+focal_length\x=587.62150
+focal_length\y=587.51184
+distortion\k1=0.01063
+distortion\k2=-0.04479
+distortion\k3=-0.00073
+distortion\k4=0.00081
+distortion\k5=0.00000
+
+[transform]
+R11=0.99994
+R12=0.00098102
+R13=0.010900
+R21=-0.00097894
+R22=1.0
+R23=-0.00019534
+R33=-0.010900
+R32=0.00018466
+R33=0.99994
+Tx=-0.02581986
+Ty=-0.0130948
+Tz=-0.0047681
diff --git a/data/kinect_petri.cal b/data/kinect_petri.cal
new file mode 100644
index 0000000000000000000000000000000000000000..4a2efc458645273988b35a72688e019fe5f5c82e
--- /dev/null
+++ b/data/kinect_petri.cal
@@ -0,0 +1,35 @@
+[rgb]
+principal_point\x=327.67398
+principal_point\y=248.59122
+focal_length\x=532.24310
+focal_length\y=531.99693
+distortion\k1=0.21059
+distortion\k2=-0.37793
+distortion\k3=-0.00167
+distortion\k4=0.00449
+distortion\k5=0.00000
+
+[depth]
+principal_point\x=305.68000
+principal_point\y=249.39797
+focal_length\x=587.79275
+focal_length\y=586.22199
+distortion\k1=-0.00565
+distortion\k2=0.01022
+distortion\k3=-0.00135
+distortion\k4=0.00066
+distortion\k5=0.00000
+
+[transform]
+R11=1.0000
+R12=0.0048
+R13=-0.0083
+R21=-0.0047
+R22=1.0000
+R23=0.0040
+R33=0.0083
+R32=-0.0039
+R33=1.0000
+Tx=-0.2832691
+Ty=-0.0514760
+Tz=0.0655281
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 17680bde41269c2c914b790e4811e0c88a01577d..7996726909d8c6c7a63b5b6cf9bde869ec5db99e 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -119,8 +119,14 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
         }
         else
         {
+            protocol_timer.stop();
             emit updateStatusString("done.");
             qDebug() << "No waypoints on UAS " << systemId;
+            current_state = WP_IDLE;
+            current_count = 0;
+            current_wp_id = 0;
+            current_partner_systemid = 0;
+            current_partner_compid = 0;
         }
     }
     else
diff --git a/waypoints.txt b/waypoints.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1d2f824a58f1248ab8e14819018a40f8714dddd3
--- /dev/null
+++ b/waypoints.txt
@@ -0,0 +1,4 @@
+QGC WPL 100
+0	0	25	0.15	0	0.15	0	1	8.54949	47.3783	552.026	0	1
+1	1	25	0.2	0	0.2	0	0	-0.15	0.2	-0.15	0.331613	1
+2	0	25	0.1	0	0.1	0	0	2	-2	-2	-0.366519	1