diff --git a/README.md b/README.md
index af56556..462d00f 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,141 @@
# oscillation_ctrl
-Repo containing oscillation damping controller for tether missions + instructions how to to set up
\ No newline at end of file
+Repo containing oscillation damping controller for tether missions + instructions how to to set up
+
+Cesar Rodriguez
+
+cesar.rodriguez@spirirobotics.com
+
+February 2022
+
+Steps to recreate stable PX4 environment + working repo
+
+## 1) Installing ROS Melodic
+
+### SETUP SOURCES.LIST
+ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+### SETUP YOUR KEYS
+ sudo apt install curl # if you haven't already installed curl
+ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+### INSTALLATION
+ sudo apt update
+ sudo apt install ros-melodic-desktop-full
+### ENVIRONMENT SETUP
+ echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+### DEPENDENCIES
+ sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
+
+Initilize rosdep:
+
+ sudo apt install python-rosdep
+ sudo rosdep init
+ rosdep update
+### PX4 DEPENDENCIES
+ sudo apt-get install python-catkin-tools python-rosinstall-generator -y
+ wstool init ~/catkin_ws/src
+### MAVLINK
+ rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall
+### MAVROS
+ rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
+### CREATE WORKSPACE AND DEPS
+ cd ~/catkin_ws
+ wstool merge -t src /tmp/mavros.rosinstall
+ wstool update -t src -j4
+ rosdep install --from-paths src --ignore-src -y
+### INSTALL GEOGRAPHIC LIB DATASETS
+ cd ~/catkin_ws
+ sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
+### BUILD SOURCE
+ cd ~/catkin_ws
+ catkin build
+
+## 2) PX4 Environment Development
+
+### DOWNLOAD PX4 SOURCE CODE
+ git clone https://github.com/PX4/PX4-Autopilot.git --recursive
+
+### RUN UBUNTU.SH
+ bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
+ #Restart computer after it is done
+
+### BUILD ROS/GAZEBO: Gets Gazebo9
+ wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
+ bash ubuntu_sim_ros_melodic.sh
+
+#### Download QGroundControl from:
+ https://docs.qgroundcontrol.com/master/en/releases/daily_builds.html
+
+#### Build jMAVSim and Gazebo
+ cd ~/PX4-Autopilot
+ make px4_sitl jmavsim
+ #May need to open QGroundControl for it to work
+ make px4_sitl gazebo
+
+#### Create px4 package
+ cd ~/PX4-Autopilot
+ DONT_RUN=1 make px4_sitl_default gazebo
+ source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
+ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
+ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
+ roslaunch px4 posix_sitl.launch
+
+## 3) oscillation_ctrl Dependencies
+### INSTALL XTERM
+ sudo apt-get update -y
+ sudo apt-get install -y xterm
+
+### INSTALL MAVROS_CONTROLLERS
+ cd ~/catkin_ws/src
+ #clone repo:
+ git clone https://github.com/Jaeyoung-Lim/mavros_controllers
+ #Download dependencies:
+ cd ~/catkin_ws
+ wstool merge -t src src/mavros_controllers/dependencies.rosinstall
+ wstool update -t src -j4
+ rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO
+ catkin build
+ source ~/catkin_ws/devel/setup.bash
+ #####Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers
+### TOOLS/SITL_GAZEBO
+ copy (or add) files in 'oscillation_ctrl/models' and 'oscillation_ctrl/worlds' to 'PX4-Autopilot/Tools/sitl_gazebo/models' and 'PX4-Autopilot/Tools/sitl_gazebo/worlds' respectively
+### ROMFS/PX4FMU_COMMON
+ copy (or add) files in 'oscillation_ctrl/airframes' to 'PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes'
+ add file name to 'CmakeLists.txt' in same 'airframe' folder (with number)
+ add airframe name in '~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake' (no number!)
+### LAUNCH FILES
+ copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
+### MAVROS
+- in px4.launch, replace:
+'''
+arg name="fcu_url" default="/dev/ttyACM0:57600"
+arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550"
+'''
+- with:
+
+ arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"
+ arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550"
+
+#### CHANGE DEVEL/SETUP.BASH
+In catkin_ws (or any working directory) add to devel/setup.bash:
+ CURRENT_DIR=$(pwd)
+ cd ~/PX4-Autopilot
+
+ source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
+ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
+ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
+
+ cd $CURRENT_DIR
+
+## JINJA TETHER FILE
+- spiri_with_tether.sdf.jinja can be altered to create desired tether model
+- changes nede to be made in px4 directory and will only take effect and
+"make px4_sitl gazebo"
+ - can do DONT_RUN=1 make px4_sitl gazebo to avoid starting px4 shell and gazebo
+- First two elements can be changed to tweak tether parameters
+ - number_elements: number of segments tether will be composed of
+ - tl: segment length (should be no shorter than 0.3 meters)
+
+
+
+
+
diff --git a/px4_launch/citadel_hill.launch b/px4_launch/citadel_hill.launch
new file mode 100644
index 0000000..fa2dd2c
--- /dev/null
+++ b/px4_launch/citadel_hill.launch
@@ -0,0 +1,58 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/px4_launch/headless_citadel_hill.launch b/px4_launch/headless_citadel_hill.launch
new file mode 100644
index 0000000..3088d6f
--- /dev/null
+++ b/px4_launch/headless_citadel_hill.launch
@@ -0,0 +1,58 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/px4_launch/headless_spiri.launch b/px4_launch/headless_spiri.launch
new file mode 100644
index 0000000..eb53023
--- /dev/null
+++ b/px4_launch/headless_spiri.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/px4_launch/headless_spiri_with_tether.launch b/px4_launch/headless_spiri_with_tether.launch
new file mode 100644
index 0000000..838859a
--- /dev/null
+++ b/px4_launch/headless_spiri_with_tether.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/px4_launch/spiri.launch b/px4_launch/spiri.launch
new file mode 100644
index 0000000..886fe8e
--- /dev/null
+++ b/px4_launch/spiri.launch
@@ -0,0 +1,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/px4_launch/spiri_with_tether.launch b/px4_launch/spiri_with_tether.launch
new file mode 100644
index 0000000..d9875d9
--- /dev/null
+++ b/px4_launch/spiri_with_tether.launch
@@ -0,0 +1,54 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/setup.txt b/setup.txt
index fedea4f..cac00aa 100644
--- a/setup.txt
+++ b/setup.txt
@@ -83,17 +83,14 @@ STEP 3) This is Cesar stuff, need to do stuff to rebuild oscillation_ctrl
catkin build
source ~/catkin_ws/devel/setup.bash
~Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers
-
-TOOLS/SITL_GAZEBO
- change default sitl_gazebo folder with working folder
- -- Change this to add model, change airframe, etc and add citadel_hill world
+ copy (or add) files in oscillation_ctrl/models and oscillation_ctrl/worlds to PX4-Autopilot/Tools/sitl_gazebo/models and PX4-Autopilot/Tools/sitl_gazebo/worlds respectively
-ROMFS/PX4FMU_COMMON
- add files in oscillation_ctrl/models and oscillation_ctrl/worlds to PX4-Autopilot/Tools/sitl_gazebo/models and PX4-Autopilot/Tools/sitl_gazebo/worlds respectively
- add files in oscillation_ctrl/airframes to PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix
+ copy (or add) files in oscillation_ctrl/airframes to PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
add file name to CmakeLists.txt in same 'airframe' folder (with number)
add airframe name in ~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake (no number!)
-LAUNCH FILES
- change default ~/PX4-Autopilot/launch folder with working folder
+ ccopy (or add) files from px4_launch directory to ~/PX4-Autopilot/launch
-MAVROS
- in px4.launch, replace:
@@ -112,6 +109,11 @@ STEP 3) This is Cesar stuff, need to do stuff to rebuild oscillation_ctrl
cd $CURRENT_DIR
+JINJA TETHER FILE
+- First two elements can be changed to tweak tether parameters
+ - number_elements: number of segments tether will be composed of
+ - tl: segment length (should be no shorter than 0.3 meters)
+