diff --git a/.gitignore b/.gitignore
index 3b19e0b..c4132b6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-config/mocap_*
+config/mocap*
launch/cortex_bridge.launch
launch/debug.launch
launch/klausen_dampen.launch
diff --git a/airframes/18.04/4000_spiri b/airframes/18.04/4000_spiri
index 60c0c1f..00d0c06 100644
--- a/airframes/18.04/4000_spiri
+++ b/airframes/18.04/4000_spiri
@@ -20,6 +20,7 @@ param set-default MPC_Z_VEL_MAX_UP 1.0
param set EKF2_AID_MASK 1
param set EKF2_HGT_MODE 0
param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
param set NAV_RCL_ACT 0
param set NAV_DLL_ACT 0
diff --git a/airframes/18.04/4001_spiri_with_tether b/airframes/18.04/4001_spiri_with_tether
index 0bb23c7..43badca 100644
--- a/airframes/18.04/4001_spiri_with_tether
+++ b/airframes/18.04/4001_spiri_with_tether
@@ -21,6 +21,9 @@ param set MPC_TKO_SPEED 1.0
param set EKF2_AID_MASK 1
param set EKF2_HGT_MODE 0
param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
+param set NAV_RCL_ACT 0
+param set NAV_DLL_ACT 0
set MIXER quad_x
set PWM_OUT 1234
diff --git a/models/spiri_with_tether/spiri_with_tether-gen.sdf b/models/spiri_with_tether/spiri_with_tether-gen.sdf
deleted file mode 100644
index 64ebeef..0000000
--- a/models/spiri_with_tether/spiri_with_tether-gen.sdf
+++ /dev/null
@@ -1,1138 +0,0 @@
-
-
-
-
- 0 0 0 0.0 0.0 0.0
-
- model://spiri
- 0.0 0.0 0.0 0.0 0.0 0
- 0
-
-
- link_spiri_attch
- spiri::base0 0 -0.15 0 0 0
-
-
-
- true
-
-
-
- 1.0
- ft_sensor_topic/spiri
- tether_to_spiri_joint
-
-
- true
- 0.15 0.0 0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_spiri_attch
-
-
- link_spiri_attch
- link_1
-
-0 0 0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
- true
-
-
- true
- 0.44999999999999996 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_1
-
-
- true
- 0.75 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_2
-
-
- true
- 1.0499999999999998 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_3
-
-
- true
- 1.3499999999999999 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_4
-
-
- true
- 1.65 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_5
-
-
- true
- 1.9499999999999997 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_6
-
-
- true
- 2.25 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_7
-
-
- true
- 2.55 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_8
-
-
- true
- 2.8499999999999996 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_9
-
- link_2
- link_1
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_3
- link_2
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_4
- link_3
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_5
- link_4
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_6
- link_5
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_7
- link_6
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_8
- link_7
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_9
- link_8
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
-
-
- 3.0 0 0 0 0 0
-
- 1
-
- 0.5
-
-
- 0 0 0 0 0 0
-
-
- 0.025
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.025
-
-
-
-
-
-
- payload
- link_9
- 0 0 0.15 0 0 0
-
-
- true
-
-
- 1.0
- ft_sensor_topic/payload
- tether_mass_connection
-
-
-
-
-
-
\ No newline at end of file
diff --git a/models/spiri_with_tether/spiri_with_tether.sdf b/models/spiri_with_tether/spiri_with_tether.sdf
index 64ebeef..873ab98 100644
--- a/models/spiri_with_tether/spiri_with_tether.sdf
+++ b/models/spiri_with_tether/spiri_with_tether.sdf
@@ -1,5 +1,5 @@
-
+
0 0 0 0.0 0.0 0.0
@@ -10,7 +10,7 @@
link_spiri_attch
- spiri::base0 0 -0.15 0 0 0
+ spiri::base0 0 -0.4 0 0 0
@@ -24,9 +24,9 @@
true
- 0.15 0.0 0 0.0 1.5707 0.0
+ 0.4 0.0 0 0.0 1.5707 0.0
- 0.0015
+ 0.004
0.01
0
@@ -40,7 +40,7 @@
0.0 0 0 0 0 0
- 0.3
+ 0.8
0.005
@@ -62,7 +62,7 @@
0 0 0 0 0 0
- 0.3
+ 0.8
0.005
@@ -73,7 +73,7 @@
- 0 0 -0.15 0 0 0
+ 0 0 -0.4 0 0 0
0.01
@@ -95,1010 +95,21 @@
0
1.4326647564469914
true
- 0.15 0.005 0.005
- 0.003
+ 0.4 0.005 0.005
+ 0.008
1.2041
0 1 0
0 0 1
link_spiri_attch
-
- link_spiri_attch
- link_1
-
-0 0 0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
- true
-
-
- true
- 0.44999999999999996 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_1
-
-
- true
- 0.75 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_2
-
-
- true
- 1.0499999999999998 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_3
-
-
- true
- 1.3499999999999999 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_4
-
-
- true
- 1.65 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_5
-
-
- true
- 1.9499999999999997 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_6
-
-
- true
- 2.25 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_7
-
-
- true
- 2.55 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_8
-
-
- true
- 2.8499999999999996 0.0 0.0 0.0 1.5707 0.0
-
- 0.0015
-
- 0.01
- 0
- 0
- 0.01
- 0
- 0.01
-
-
-
- 0.0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
- 0.00005
-
-
-
-
- 1.0
- 1.0
-
-
-
-
-
- 0 0 0 0 0 0
-
-
- 0.3
- 0.005
-
-
-
-
-
-
- 0 0 -0.15 0 0 0
-
-
- 0.01
-
-
-
-
-
-
-
-
-
- 0
- 0
- 1.253582
- 0
- 1.4326647564469914
- true
- 0.15 0.005 0.005
- 0.003
- 1.2041
- 0 1 0
- 0 0 1
- link_9
-
- link_2
- link_1
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_3
- link_2
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_4
- link_3
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_5
- link_4
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_6
- link_5
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_7
- link_6
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_8
- link_7
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
- link_9
- link_8
-
-0 0 -0.15 0 0 0
-
- 0 1 0
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
- 0 0 1
-
- 0.05
- 0.0
- 0.05
- 0.0
-
- true
-
-
-
- 1
-
-
-
+
- 3.0 0 0 0 0 0
+ 0.8 0 0 0 0 0
1
- 0.5
+ 0.25
0 0 0 0 0 0
@@ -1117,22 +128,60 @@
-
-
+
+
payload
- link_9
- 0 0 0.15 0 0 0
+ link_spiri_attch
+ 0 0 0.4 0 0 0
true
-
- 1.0
- ft_sensor_topic/payload
- tether_mass_connection
-
-
+
+
+ 0.8 0 0 0 0 0
+
+ 0.015
+
+ 1e-05
+ 0
+ 0
+ 1e-05
+ 0
+ 1e-05
+
+
+
+
+ pload_imu
+ payload
+
+ 1 0 0
+
+ 0
+ 0
+ 0
+ 0
+
+
+ 0
+ 0
+
+ 0
+
+
+
+
+ true
+ pload_imu
+ /pload_imu
+ imu_service
+ 0.0
+ 20.0
+
+
+
-
\ No newline at end of file
+
diff --git a/models/spiri_with_tether/spiri_with_tether.sdf.jinja b/models/spiri_with_tether/spiri_with_tether.sdf.jinja
index 6714efd..9ffd63c 100644
--- a/models/spiri_with_tether/spiri_with_tether.sdf.jinja
+++ b/models/spiri_with_tether/spiri_with_tether.sdf.jinja
@@ -4,28 +4,29 @@
{#--------------------------------The parameters bellow are the ones to be tweaked---------------------------#}
{#--------------------------------------------:Tweakable parameters:-----------------------------------------#}
{%- set number_elements = 1 -%}
-{%- set tl = 1 -%} {#- tl: length of the tether element (meters) -#}
-{%- set cr = 0.005 -%} {#- cr: radius of the tether element (meters) -#}
-{%- set cr_v = 0.005 -%} {#- cr_v: radius of the tether element visual (meters) -#}
-{%- set sr = 0.01 -%} {#- sr: element joint radius (sphere) (meters) -#}
-{%- set m = 0.005 * tl -%} {#- m: mass of the element (kg), given the length -#}
-{%- set cda = 1.253582 -%} {#- cda: the ratio of the drag coefficient before stall. -#}
+{%- set tl = 0.8 -%} {#- tl: length of the tether element (meters) -#}
+{%- set cr = 0.005 -%} {#- cr: radius of the tether element (meters) -#}
+{%- set cr_v = 0.005 -%} {#- cr_v: radius of the tether element visual (meters) -#}
+{%- set sr = 0.01 -%} {#- sr: element joint radius (sphere) (meters) -#}
+{%- set m = 0.005 * tl -%} {#- m: mass of the element (kg), given the length -#}
+{%- set cda = 1.253582 -%} {#- cda: the ratio of the drag coefficient before stall. -#}
{%- set cda_stall = 1.4326647564469914 -%} {#- cda_stall: the ratio of the drag coefficient after stall. -#}
-{%- set damping = 0.05 -%} {#- Model damping -#}
-{%- set friction = 0.0 -%} {#- Friction of the model relative to the world -#}
-{%- set tether_stiffness = 0.05 -%} {#- Tether stiffness -#}
-{%- set joint_stiffness = 0.05 -%} {#- Joint stiffness, i.e where tether attaches to drone -#}
-{%- set spring_reference = 0.0 -%} {#- Reference where the spring forces are applied -#}
-{%- set element_color = 'White' -%} {#- Color of the tether elements -#}
-{%- set joint_color = 'Red' -%} {#- Color of the tether element joints -#}
-{%- set vehicle_spawn_point_y = 0.0 -%} {#- Y coordinate of vehicle spawn point -#}
-{%- set debug = 0 -%} {#- Used for debugging tether joints -#}
+{%- set damping = 0.0001 -%} {#- Model damping -#}
+{%- set friction = 0.0 -%} {#- Friction of the model relative to the world -#}
+{%- set tether_stiffness = 0.0005 -%} {#- Tether stiffness -#}
+{%- set joint_stiffness = 0.0005 -%} {#- Joint stiffness, i.e where tether attaches to drone -#}
+{%- set spring_reference = 0.0 -%} {#- Reference where the spring forces are applied -#}
+{%- set element_color = 'White' -%} {#- Color of the tether elements -#}
+{%- set joint_color = 'Red' -%} {#- Color of the tether element joints -#}
+{%- set vehicle_spawn_point_y = 0.0 -%} {#- Y coordinate of vehicle spawn point -#}
+{%- set debug = 0 -%} {#- Used for debugging tether joints -#}
{#----------------------------------------------------------------------------------------------------------#}
{#---------------------------------------------:Payload parameters:-----------------------------------------#}
{#----------------------------------------------------------------------------------------------------------#}
-{%- set payload_m = 0.5 -%} {#- Payload mass (kg) -#}
-{%- set pr = 0.025 -%} {#- Payload radius (meters). Has to be less than FULL tether length -#}
+{%- set payload_m = 0.25 -%} {#- Payload mass (kg). Used to be 0.5 -#}
+{%- set pr = 0.025 -%} {#- Payload radius (meters). Has to be less than FULL tether length -#}
+{%- set add_imu = 1 -%} {#- Add IMU to payload for localization-#}
{#-----------------------------------------------------------------------------------------------------------#}
{#----------------------------------------------:Spiri parameters:-------------------------------------------#}
{#-----------------------------------------------------------------------------------------------------------#}
@@ -62,7 +63,7 @@
{%- set pload_joint_z = tl/2 -%}
{%- endif -%}
-{%- set tether_pos = vehicle_spawn_point_z - tl/2 -%} {#- Spawns tether vertically -#}
+{%- set tether_pos = vehicle_spawn_point_z - full_tether_len/2 -%} {#- Spawns tether vertically -#}
{#----------------------------------------------------------------------------------------------------------#}
{#-----------------------------------------------:MACROS:---------------------------------------------------#}
{#----------------------------------------------------------------------------------------------------------#}
@@ -114,7 +115,7 @@
tether_mass_connection
{%- endif %}
-
+
{%- endmacro -%}
{%- macro cylinder(tl, cr_v) -%}
@@ -126,6 +127,51 @@
{%- endmacro -%}
+{%- macro imu_link(x, y, z) -%}
+
+ {{ x }} {{ y }} {{ z }} 0 0 0
+
+ 0.015
+
+ 1e-05
+ 0
+ 0
+ 1e-05
+ 0
+ 1e-05
+
+
+
+
+ pload_imu
+ payload
+
+ 1 0 0
+
+ 0
+ 0
+ 0
+ 0
+
+
+ 0
+ 0
+
+ 0
+
+
+
+
+ true
+ pload_imu
+ /pload_imu
+ imu_service
+ 0.0
+ 20.0
+
+
+{%- endmacro -%}
+
{%- macro sphere(sr) -%}
@@ -357,7 +403,7 @@
0 0 0 0.0 0.0 0.0 {# only have to change this to change spawn location #}
- model://spiri
+ model://spiri_mocap
0.0 0.0 {{ vehicle_spawn_point_z }} 0.0 0.0 {{ yaw }}
{{ debug }}
@@ -385,6 +431,11 @@
{%- else -%}
{%- set n = number_elements - 1 -%}
{% endif %} {{ payload(payload_m,n,pr,pload_spawn_x,pload_spawn_y,pload_spawn_z,pload_joint_x,pload_joint_y,pload_joint_z) }}
+
+{#------------- ADD IMU -----------#}
+ {%- if add_imu == 1 -%}
+ {{ imu_link(pload_spawn_x,pload_spawn_y,pload_spawn_z) }}
+ {% endif %}
diff --git a/px4_setup/px4_bash.sh b/px4_setup/px4_bash.sh
new file mode 100644
index 0000000..16e7299
--- /dev/null
+++ b/px4_setup/px4_bash.sh
@@ -0,0 +1,9 @@
+echo "Copying model files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
+echo "Copying world files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
+echo "Copying airframes files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/18.04/* ~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
+echo "Adding airframe names to px4_sitl make targets..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
+echo "Done!"
diff --git a/src/path_follow.cpp b/src/path_follow.cpp
index 1b922f7..7afb312 100644
--- a/src/path_follow.cpp
+++ b/src/path_follow.cpp
@@ -143,7 +143,7 @@ int main(int argc, char **argv)
if (waypoint_cl.call(wpoint_srv))
{
ROS_INFO("Waypoints received");
- // populate desired waypoints - this is only for original hover as
+ // populate desired waypoints
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
@@ -167,11 +167,14 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
}
}
-
- if(current_state.mode == "OFFBOARD" && current_state.armed){
- ROS_INFO_ONCE("Spiri is taking off");
+ if (waypoint_cl.call(wpoint_srv))
+ {
+ // check if waypoints have changed
+ buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+ buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+ buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+ alt_des = wpoint_srv.response.xd.z;
}
-
// Check if we want to use oscillation controller
if (ros::param::has("/status/use_ctrl")){
ros::param::get("/status/use_ctrl", oscillation_damp);
@@ -186,7 +189,7 @@ int main(int argc, char **argv)
// Publish position setpoints
local_pos_pub.publish(buff_pose);
}
- ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
+ ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
ROS_INFO("---------------------------");
} else {
diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py
index 6c7382b..da56e7a 100755
--- a/src/wpoint_tracker.py
+++ b/src/wpoint_tracker.py
@@ -25,19 +25,12 @@ class Main:
self.param_exists = False # check in case param does not exist
while self.param_exists == False:
- if rospy.has_param('sim/waypoints'):
- self.wpoints = rospy.get_param('sim/waypoints') # get waypoints
+ if rospy.has_param('status/waypoints'):
+ self.wpoints = rospy.get_param('status/waypoints') # get waypoints
self.xd.x = self.wpoints['x']
self.xd.y = self.wpoints['y']
self.xd.z = self.wpoints['z']
self.param_exists = True
- elif rospy.has_param('mocap/waypoints'):
- self.wpoints = rospy.get_param('mocap/waypoints') # get waypoints
- self.xd.x = self.wpoints['x']
- self.xd.y = self.wpoints['y']
- self.xd.z = self.wpoints['z']
- self.param_exists = True
-
elif rospy.get_time() - self.tstart >= 5.0:
self.xd.x = 0.0
self.xd.y = 0.0