.gitignore

@@ -1,7 +1,7 @@
 config/mocap*
 launch/cortex_bridge.launch
-launch/headless_spiri_mocap.launch
-launch/headless_spiri_with_tether_mocap.launch
+launch/debug.launch
+launch/klausen_dampen.launch
 launch/mocap_*
 src/development_*
 src/killswitch_client.py
@@ -9,9 +9,10 @@ src/land_client.py
 src/MoCap_*.py
 src/Mocap_*.py
 src/mocap_*
+src/segmented_tether.py
+src/segmented_tether_fast.py
 msg/Marker.msg
 msg/Markers.msg
 *.rviz
 setup.txt
-px4_setup/protobuf_install.txt
 

CMakeLists.txt

@@ -84,11 +84,17 @@ target_link_libraries(pathFollow_node ${catkin_LIBRARIES})
 add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
 #add_dependencies(pathFollow_node oscillation_ctrl_generate_messages_cpp)
 
-add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
+#add_executable(mocap_offb_node src/mocap_offb_node.cpp)
 
-target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
+#target_link_libraries(mocap_offb_node ${catkin_LIBRARIES})
 
-add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+
+#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
+
+#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
+
+#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}

launch/mocap_sim.launch

@@ -56,9 +56,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 	<!-- PX4 LAUNCH -->
 	<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
 	</include>
-	
-	<!-- Tf LAUNCH -->
-	<node pkg="tf" type="static_transform_publisher" name="map_to_pload_imu" args="0 0 0 0 0 0 map pload_imu 10" />
-	<node pkg="tf" type="static_transform_publisher" name="map_to_pload" args="0 0 0 0 0 0 map payload 10" />
 </launch>
 

launch/takeoff_noCtrl.launch

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="model" default="headless_spiri_with_tether"/>
+  <arg name='test' default="none"/>
+  
+	<group ns="sim">
+  	<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
+	</group>
+  <group ns="status">
+		<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
+  	<param name="test_type" value="$(arg test)"/>
+  </group>
+
+	<node
+		pkg="oscillation_ctrl"
+		type="LinkState.py"
+		name="LinkStates"
+		launch-prefix="xterm -e"
+	/>
+	<node
+		pkg="oscillation_ctrl"
+		type="offb_node"
+		name="offb_node"
+		launch-prefix="xterm -e"
+	/>
+
+
+	<node
+		pkg="oscillation_ctrl"
+		type="wpoint_tracker.py"
+		name="waypoints_server"
+	/>
+
+	<group if="$(eval test != 'none')">
+		<node
+			pkg="oscillation_ctrl"
+			type="perform_test.py"
+			name="test_node"
+			launch-prefix="xterm -e"
+		/>
+	</group>
+	<!-- PX4 LAUNCH -->
+	<include file="$(find px4)/launch/$(arg model).launch"/>
+</launch>

src/MoCap_Localization_fake.py

@@ -200,11 +200,7 @@ class Main:
 		# fill out necesssary fields
 		self.drone_pose.header.frame_id = "/map"
 		self.drone_pose.header.stamp = rospy.Time.now()
-		# load angles
 		self.load_angles.header.stamp = rospy.Time.now()
-		# twobody status
-		self.twobody_status.header.frame_id = "/map"
-		self.twobody_status.header.stamp = rospy.Time.now()
 		# publish
 		self.visionPose_pub.publish(self.drone_pose)  # publish pose to mavros
 		self.loadAng_pub.publish(self.load_angles)    # publish load angles to controller

src/klausen_control.py

@@ -15,7 +15,7 @@ import rosservice
 
 from tf.transformations   import *
 from scipy.integrate  	  import odeint 
-from oscillation_ctrl.msg import RefSignal, LoadAngles
+from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus
 from oscillation_ctrl.srv import WaypointTrack
 from geometry_msgs.msg 	  import Pose, Point, TwistStamped, PoseStamped
 from sensor_msgs.msg 	  import Imu
@@ -185,8 +185,7 @@ class Main:
 		if rospy.has_param('status/hover_throttle'):
 			hover_throttle = rospy.get_param('status/hover_throttle')
 		else:
-			hover_throttle = (self.tot_m*9.81 + 11.2)/34.84 # linear scaling depending on dependent on mass
-			rospy.set_param('status/hover_throttle',hover_throttle)
+			hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass
 		kf = hover_throttle/(self.tot_m*9.81)
 		return kf
 

src/mocap_path_follow.cpp

@@ -182,14 +182,8 @@ int main(int argc, char **argv)
 					takeoff = true;
 				}
 			}
-			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::get("/use_ctrl", oscillation_damp) == true){
 			if (ros::param::has("/status/use_ctrl")){
 				ros::param::get("/status/use_ctrl", oscillation_damp);
 				if(oscillation_damp){