diff --git a/.gitignore b/.gitignore
index 8e75294..25d96a3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,10 +3,10 @@ launch/mocap_*
launch/cortex_bridge.launch
src/MoCap_Localization_*.py
src/Mocap_*.py
+src/segmented_tether.py
src/killswitch_client.py
src/land_client.py
msg/Marker.msg
msg/Markers.msg
-launc
*.rviz
diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml
index c2ac39d..30358c6 100644
--- a/config/Ctrl_param.yaml
+++ b/config/Ctrl_param.yaml
@@ -1,5 +1,5 @@
# Ros param when using Klausen Ctrl
-wait: 52
+wait: 40
waypoints: {x: 0.0, y: 0.0, z: 5.0}
diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml
index 4402a42..27c4041 100644
--- a/config/px4_pluginlists.yaml
+++ b/config/px4_pluginlists.yaml
@@ -1,5 +1,6 @@
plugin_blacklist:
# common
+#- distance_sensor
- safety_area
# extras
- image_pub
@@ -12,5 +13,6 @@ plugin_blacklist:
- odometry
- px4flow
-plugin_whitelist: []
+plugin_whitelist: []
#- 'sys_*'
+
diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch
index c3896a6..09a685a 100644
--- a/launch/cortex_bridge.launch
+++ b/launch/cortex_bridge.launch
@@ -11,6 +11,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
+
+
+
+
+
+
+
+
-
+
+
+
-
diff --git a/launch/headless_spiri.launch b/launch/headless_spiri.launch
new file mode 100644
index 0000000..f2dba89
--- /dev/null
+++ b/launch/headless_spiri.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/headless_spiri_with_tether.launch b/launch/headless_spiri_with_tether.launch
new file mode 100644
index 0000000..1c0bb7e
--- /dev/null
+++ b/launch/headless_spiri_with_tether.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/offboard.launch b/launch/offboard.launch
deleted file mode 100644
index 2512d05..0000000
--- a/launch/offboard.launch
+++ /dev/null
@@ -1,7 +0,0 @@
-
-
-
-
-
-
-
diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch
index 07c0af2..0e5662d 100644
--- a/launch/oscillation_damp.launch
+++ b/launch/oscillation_damp.launch
@@ -4,7 +4,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
-
+
@@ -14,6 +14,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
+
+
-
+
diff --git a/launch/path_follow.launch b/launch/path_follow.launch
deleted file mode 100644
index 50f1d1d..0000000
--- a/launch/path_follow.launch
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
-
-
-
-
diff --git a/launch/px4.launch b/launch/px4.launch
index 777c00c..c182acd 100644
--- a/launch/px4.launch
+++ b/launch/px4.launch
@@ -8,10 +8,14 @@
+
+
+
+
-
-
+
+
diff --git a/launch/spiri.launch b/launch/spiri.launch
new file mode 100644
index 0000000..fa046dd
--- /dev/null
+++ b/launch/spiri.launch
@@ -0,0 +1,55 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/spiri_with_tether.launch b/launch/spiri_with_tether.launch
new file mode 100644
index 0000000..4da8c16
--- /dev/null
+++ b/launch/spiri_with_tether.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/klausen_control.py b/src/klausen_control.py
index 13a88a3..3b905d4 100755
--- a/src/klausen_control.py
+++ b/src/klausen_control.py
@@ -103,13 +103,13 @@ class Main:
# gains for thrust PD Controller
#self.Kp = 2.7
#self.Kd = 3
- self.Kp = 2.7
+ self.Kp = 3.0
self.Kd = 3
self.max_a = 14.2
- self.max_t = self.drone_m*self.max_a
+ self.max_t = self.tot_m*self.max_a
self.R = np.empty([3,3]) # rotation matrix
self.e3 = np.array([[0],[0],[1]])
- self.thrust_offset = 0.7 # There was found to be a constant offset
+ self.thrust_offset = 1.0 # There was found to be a constant offset 0.7
# --------------------------------------------------------------------------------#
# SUBSCRIBERS
# --------------------------------------------------------------------------------#
@@ -266,7 +266,7 @@ class Main:
#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t
#thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t
#thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t
- thrust_vector = (9.81*self.drone_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
+ thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
#thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
#thrust = thrust_vector[2]
@@ -370,7 +370,7 @@ class Main:
# Desired body-oriented forces
# shouldnt it be tot_m*path_acc?
- Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p))
+ Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,0.5*self.dt*(self.z1 - self.z1_p))
# Update self.z1_p for "integration"
self.z1_p = self.z1
diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py
index 8a8ae7d..bfdfc6a 100755
--- a/src/ref_signalGen.py
+++ b/src/ref_signalGen.py
@@ -83,7 +83,7 @@ class Main:
self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation
- self.w_tune = 3.13 # 3.13 works well? #########################################################################
+ self.w_tune = 3 # 3.13 works well? #########################################################################
self.epsilon = 0.7 # Damping ratio
# need exception if we do not have tether:
@@ -117,7 +117,7 @@ class Main:
self.z0 = [0, 0, 0, 0]
# Constants for feedback
- self.Gd = 0.325
+ self.Gd = 0.325 #0.325
self.td = 2*self.Gd*math.pi/self.wd
# Constants for shape filter/ Input shaping
@@ -258,7 +258,7 @@ class Main:
self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot
self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1]
- self.phi_fb[self.FB_idx] = self.load_angles.phi
+ self.phi_fb[self.FB_idx] = self.load_angles.phi
self.phi_vel_fb[self.FB_idx] = self.load_angles.phidot
self.phi_acc_fb[self.FB_idx] = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1]
diff --git a/src/square.py b/src/square.py
deleted file mode 100755
index 88af1eb..0000000
--- a/src/square.py
+++ /dev/null
@@ -1,88 +0,0 @@
-#!/usr/bin/env python2.7
-
-
-### Cesar Rodriguez June 2021
-### Script to generate set points which form a square with 2m side lengths
-import rospy, tf
-import time
-from geometry_msgs.msg import Point
-from std_msgs.msg import Bool
-
-class Main:
- # class method used to initialize variables once when the program starts
- def __init__(self):
-
- # variable(s)
- self.Point = Point()
- # Init x, y, & z coordinates
- self.Point.x = 0
- self.Point.y = 0
- self.Point.z = 3.5
-
- self.xarray = [1,2,2,2,1,0,0]
- self.yarray = [0,0,1,2,2,2,1]
- self.i = 0
- self.j = 0
- self.buffer = 10
- self.bool = False
-
- self.param_exists = False # check in case param does not exist
- while self.param_exists == False:
- if rospy.has_param('status/test_type'):
- self.test = rospy.get_param('status/test_type')
- self.param_exists = True
-
- elif rospy.get_time() - self.tstart >= 3.0 || rospy.get_param('status/test_type') == 'none'
- self.test = 'none'
- break
-
-
- # subscriber(s), with specified topic, message type, and class callback method
- rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)
-
- # publisher(s), with specified topic, message type, and queue_size
- self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5)
-
- # rate(s)
- pub_rate = 1 # rate for the publisher method, specified in Hz
-
- # timer(s), used to control method loop frequencies as defined by the rate(s)
- self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)
-
- def wait_cb(self,data):
- self.bool = data
-
- # Publish messages
- def pub(self,pub_timer):
- if self.bool == False:
- rospy.loginfo('Waiting...')
- else:
- # Check if i is too large. loop back to first point
- if self.i >= len(self.xarray):
- self.Point.x = 0
- self.Point.y = 0
-
- else:
- self.Point.x = self.xarray[self.i]
- self.Point.y = self.yarray[self.i]
-
- rospy.loginfo("Sending [Point x] %d [Point y] %d",
- self.Point.x, self.Point.y)
-
- # Published desired msgs
- # self.Point.header.stamp = rospy.Time.now()
- self.pub_square.publish(self.Point)
- self.j += 1
- self.i = self.j // self.buffer
-
-if __name__=="__main__":
-
- # Initiate ROS node
- rospy.init_node('square',anonymous=True)
- try:
- Main() # create class object
- rospy.spin() # loop until shutdown signal
-
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/step.py b/src/step.py
deleted file mode 100755
index 6a58afc..0000000
--- a/src/step.py
+++ /dev/null
@@ -1,62 +0,0 @@
-#!/usr/bin/env python2.7
-
-### Cesar Rodriguez Dec 2021
-### Generate step input in x or y direction
-import rospy, tf
-import time
-from geometry_msgs.msg import Point
-from std_msgs.msg import Bool
-
-class Main:
- # class method used to initialize variables once when the program starts
- def __init__(self):
-
- # variable(s)
- self.Point = Point()
- # Init x, y, & z coordinates
- self.Point.x = 5
- self.Point.y = 0
- self.Point.z = 3.5 # need to check what the xd was previously and save the height
-
- self.bool = False
-
- # subscriber(s)
- rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)
-
- # publisher(s), with specified topic, message type, and queue_size
- self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5)
-
- # rate(s)
- pub_rate = 1 # rate for the publisher method, specified in Hz
-
- # timer(s), used to control method loop frequencies as defined by the rate(s)
- self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)
-
- # Callbacks
- def wait_cb(self,data):
- self.bool = data
-
- # Publish messages
- def pub(self,pub_timer):
- if self.bool == False:
- rospy.loginfo('Waiting...')
- else:
- #self.Point.header.stamp = rospy.Time.now()
-
- # Published desired msgs
- self.pub_step.publish(self.Point)
-
- rospy.loginfo("Sending [Point x] %d [Point y] %d",
- self.Point.x, self.Point.y)
-
-if __name__=="__main__":
-
- # Initiate ROS node
- rospy.init_node('step',anonymous=True)
- try:
- Main() # create class object
- rospy.spin() # loop until shutdown signal
-
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/waypoint_determine.py b/src/waypoint_determine.py
index 70029d9..b6470a4 100755
--- a/src/waypoint_determine.py
+++ b/src/waypoint_determine.py
@@ -44,7 +44,7 @@ class Main:
def wp_input(self):
# testing:
- self.lat = input('Enter lat: ')
+ self.lat = input('Enter lat: ')
self.lon = input('Enter lon: ')
self.lat = 44.6480876