commit 987116130cae8b17f511c8da908bf6e1780c956c
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Tue Apr 17 12:35:50 2012 -0400

    Moved mikrokopter package one folder deeper
    
    The packages arduino and altitude_node aren't found inside another
    package directory, so this way they're both found and the repo is a
    little more organized.

diff --git a/mikrokopter/CMakeLists.txt b/mikrokopter/CMakeLists.txt
deleted file mode 100644
index 70b1b7e..0000000
--- a/mikrokopter/CMakeLists.txt
+++ /dev/null
@@ -1,33 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type.  Options are:
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)
-target_link_libraries(${PROJECT_NAME} serial)
-
-rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
-rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp)
-target_link_libraries(keyboard_control ncurses)
-rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp)
-rosbuild_add_executable(node_control src/node_control.cpp src/nav_lib.cpp)
-rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ../control/pid_control.cpp)
-rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)
diff --git a/mikrokopter/Makefile b/mikrokopter/Makefile
deleted file mode 100644
index b75b928..0000000
--- a/mikrokopter/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/mikrokopter/include/mikrokopter/interface.h b/mikrokopter/include/mikrokopter/interface.h
deleted file mode 100644
index e5cb347..0000000
--- a/mikrokopter/include/mikrokopter/interface.h
+++ /dev/null
@@ -1,154 +0,0 @@
- /*!
-  *  \file    interface.h
-  *  \brief   Defines the MikrokopterInterface class
-  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
-  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
-  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
-  *  \date    2011 October 25
-  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
-  */
-
-#ifndef _MIKROKOPTER_INTERFACE_H_
-#define _MIKROKOPTER_INTERFACE_H_
-
-#include <SerialStream.h>
-using namespace LibSerial;
-
-#include <string>
-#include <ros/ros.h>
-
-//Messages types:
-//Command Output
-#include <std_msgs/Empty.h>
-#include <mikrokopter/PPM.h>
-#include <mikrokopter/NcDebug.h>
-#include <mikrokopter/FcDebug.h>
-#include <mikrokopter/Version.h>
-#include <mikrokopter/AnalogLabel.h>
-#include <mikrokopter/Control.h>
-#include <mikrokopter/Navi.h>
-#include <mikrokopter/AngleOut.h>
-
-namespace CA
-{
-  class MikrokopterInterface
-  {
-  public:
-    MikrokopterInterface(ros::NodeHandle nh);
-    virtual ~MikrokopterInterface();
-    enum Address
-    {
-      BlankAddress  = 'a',
-      FcAddress     = 'b',
-      NcAddress     = 'c',
-      Mk3MagAddress = 'd',
-      BlCtrlAddress = 'f'
-    };
-  protected:
-  
-  
-  private:
-    
-    // Member variables  
-    SerialStream fd;
-    std::vector<unsigned char> buffer;
-    std::map<unsigned char, unsigned int> msg_len_table;
-    ros::NodeHandle n;
-    
-    // Publishers
-    ros::Publisher ppm_data_pub;
-    ros::Publisher nc_debug_data_pub;
-    ros::Publisher fc_debug_data_pub;
-    ros::Publisher version_data_pub;
-    ros::Publisher label_data_pub;
-    ros::Publisher set_control_ack_pub;  
-    ros::Publisher control_data_pub;
-    ros::Publisher navi_data_pub;
-    ros::Publisher angleout_data_pub;
-    
-    // Subscribers
-    ros::Subscriber get_control_sub;
-    ros::Subscriber set_control_sub;
-    ros::Subscriber req_reset_sub;
-    
-    // Timers
-    ros::Timer readSerialTimer;
-    ros::Timer req_version_timer;
-    ros::Timer req_analog_label_timer;
-    ros::Timer req_nc_debug_timer;
-    ros::Timer req_fc_debug_timer;
-    ros::Timer req_navi_timer;
-    ros::Timer req_external_control_timer;
-    
-    // Parameters
-    double req_version_rate;
-    int    req_version_addr;
-
-    double req_analog_label_rate;
-    int    req_analog_label_idx;
-
-    double req_fc_debug_rate;
-    double req_nc_debug_rate;
-    double req_navi_rate;
-    double req_reset_rate;
-    double req_external_control_rate;
-
-    //
-    // Callback functions
-    //
-    void readSerialCallback(const ros::TimerEvent&);
-    void reqVersionCallback(const ros::TimerEvent&);
-    void reqAnalogLabelCallback(const ros::TimerEvent&);
-    void reqNcDebugCallback(const ros::TimerEvent&);
-    void reqFcDebugCallback(const ros::TimerEvent&);
-    void reqNaviCallback(const ros::TimerEvent&);
-    void reqResetCallback(const std_msgs::Empty& msg);
-    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);
-    void getControlCallback(const ros::TimerEvent&);
-    
-    //
-    // Message field extraction functions
-    //
-    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);
-    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);
-    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);
-    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);
-    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);
-    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);
-    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);
-    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);
-    
-    //
-    // Message handling functions
-    //
-    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);
-
-    //
-    // Utility functions
-    //
-    void redirectSerialTo(MikrokopterInterface::Address addr);
-    
-    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);
-    int calculateChecksum(std::vector<unsigned char>& buffer);
-    int calculateChecksum(unsigned char* buffer, int len);
-    bool isValidChecksum(std::vector<unsigned char>& buffer);
-    void updateWithChecksum(unsigned char* buffer, int len);
-    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);
-    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);
-    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);
-    uint8_t connectedToAddress;
-  };
-  
-}
-
-
-
-#endif
diff --git a/mikrokopter/launch/joystick.launch b/mikrokopter/launch/joystick.launch
deleted file mode 100644
index bee1307..0000000
--- a/mikrokopter/launch/joystick.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-  <node name="joy_node" pkg="joy" type="joy_node">
-  </node>
-  <node name="joystick_control" pkg="mikrokopter" type="joystick_control">
-    <!-- rate in Hz to publish Control messages -->
-    <param name="publish_rate" value="20" />
-  </node>
-</launch>
-
-
diff --git a/mikrokopter/launch/mikrokopter.launch b/mikrokopter/launch/mikrokopter.launch
deleted file mode 100644
index 09a9f72..0000000
--- a/mikrokopter/launch/mikrokopter.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-<launch>
-
-  <node name="mikrokopter" pkg="mikrokopter" 	type="mikrokopter">
-    
-    <param name="port" value="/dev/ttyUSB0" />
-    	
-    <param name="req_version_rate" value="0.0" />	
-    <param name="req_version_addr" value="97" />
-    
-    <param name="req_analog_label_rate" value="0.0" />
-    <param name="req_nc_debug_rate" value="0.0" />
-    <param name="req_fc_debug_rate" value="10.0" />
-    <param name="req_navi_rate" value="0.0" />
-    <param name="req_external_control_rate" value="0.0" />
-    
-  </node>
-       
-</launch>
diff --git a/mikrokopter/launch/onboard.launch b/mikrokopter/launch/onboard.launch
deleted file mode 100644
index b5a6079..0000000
--- a/mikrokopter/launch/onboard.launch
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-  <include file="$(find mikrokopter)/launch/mikrokopter.launch" />
-  <node name="mk_wrapper" pkg="mikrokopter" type="mk_wrapper">
-    <!-- if no Control message is received in this time in ms, go dead-->
-    <param name="max_quiet_time" value="200" />
-  </node>
-</launch>
diff --git a/mikrokopter/mainpage.dox b/mikrokopter/mainpage.dox
deleted file mode 100644
index 024b5c5..0000000
--- a/mikrokopter/mainpage.dox
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b mikrokopter is ... 
-
-<!-- 
-Provide an overview of your package.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
-*/
diff --git a/mikrokopter/manifest.xml b/mikrokopter/manifest.xml
deleted file mode 100644
index 7ffd9b2..0000000
--- a/mikrokopter/manifest.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-<package>
-  <description brief="mikrokopter">
-
-     mikrokopter
-
-  </description>
-  <author>Andrew Chambers/achamber@gmail.com, Justin Haines/jhaines@andrew.cmu.edu, Sebastian Scherer/basti@andrew.cmu.edu</author>
-  <license>GPLv3</license>
-  <review status="unreviewed" notes=""/>
-  <url>https://aeroscoutsim.frc.cs.cmu.edu/redmine/projects/riverine/wiki/Mikrokopter</url>
-  <depend package="std_msgs" />
-  <depend package="sensor_msgs" />
-  <depend package="roscpp" />
-  <rosdep name="libserial" />
-
-</package>
-
-
diff --git a/mikrokopter/mikrokopter/CMakeLists.txt b/mikrokopter/mikrokopter/CMakeLists.txt
new file mode 100644
index 0000000..1b5ab5f
--- /dev/null
+++ b/mikrokopter/mikrokopter/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+set(CONTROL_PATH ../../control)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+include_directories(${CONTROL_PATH})
+
+rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)
+target_link_libraries(${PROJECT_NAME} serial)
+
+rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
+rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp)
+target_link_libraries(keyboard_control ncurses)
+rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp)
+rosbuild_add_executable(node_control src/node_control.cpp src/nav_lib.cpp)
+rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ${CONTROL_PATH}/pid_control.cpp)
+rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)
diff --git a/mikrokopter/mikrokopter/Makefile b/mikrokopter/mikrokopter/Makefile
new file mode 100644
index 0000000..b75b928
--- /dev/null
+++ b/mikrokopter/mikrokopter/Makefile
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/mikrokopter/mikrokopter/include/mikrokopter/interface.h b/mikrokopter/mikrokopter/include/mikrokopter/interface.h
new file mode 100644
index 0000000..e5cb347
--- /dev/null
+++ b/mikrokopter/mikrokopter/include/mikrokopter/interface.h
@@ -0,0 +1,154 @@
+ /*!
+  *  \file    interface.h
+  *  \brief   Defines the MikrokopterInterface class
+  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
+  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
+  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
+  *  \date    2011 October 25
+  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
+  */
+
+#ifndef _MIKROKOPTER_INTERFACE_H_
+#define _MIKROKOPTER_INTERFACE_H_
+
+#include <SerialStream.h>
+using namespace LibSerial;
+
+#include <string>
+#include <ros/ros.h>
+
+//Messages types:
+//Command Output
+#include <std_msgs/Empty.h>
+#include <mikrokopter/PPM.h>
+#include <mikrokopter/NcDebug.h>
+#include <mikrokopter/FcDebug.h>
+#include <mikrokopter/Version.h>
+#include <mikrokopter/AnalogLabel.h>
+#include <mikrokopter/Control.h>
+#include <mikrokopter/Navi.h>
+#include <mikrokopter/AngleOut.h>
+
+namespace CA
+{
+  class MikrokopterInterface
+  {
+  public:
+    MikrokopterInterface(ros::NodeHandle nh);
+    virtual ~MikrokopterInterface();
+    enum Address
+    {
+      BlankAddress  = 'a',
+      FcAddress     = 'b',
+      NcAddress     = 'c',
+      Mk3MagAddress = 'd',
+      BlCtrlAddress = 'f'
+    };
+  protected:
+  
+  
+  private:
+    
+    // Member variables  
+    SerialStream fd;
+    std::vector<unsigned char> buffer;
+    std::map<unsigned char, unsigned int> msg_len_table;
+    ros::NodeHandle n;
+    
+    // Publishers
+    ros::Publisher ppm_data_pub;
+    ros::Publisher nc_debug_data_pub;
+    ros::Publisher fc_debug_data_pub;
+    ros::Publisher version_data_pub;
+    ros::Publisher label_data_pub;
+    ros::Publisher set_control_ack_pub;  
+    ros::Publisher control_data_pub;
+    ros::Publisher navi_data_pub;
+    ros::Publisher angleout_data_pub;
+    
+    // Subscribers
+    ros::Subscriber get_control_sub;
+    ros::Subscriber set_control_sub;
+    ros::Subscriber req_reset_sub;
+    
+    // Timers
+    ros::Timer readSerialTimer;
+    ros::Timer req_version_timer;
+    ros::Timer req_analog_label_timer;
+    ros::Timer req_nc_debug_timer;
+    ros::Timer req_fc_debug_timer;
+    ros::Timer req_navi_timer;
+    ros::Timer req_external_control_timer;
+    
+    // Parameters
+    double req_version_rate;
+    int    req_version_addr;
+
+    double req_analog_label_rate;
+    int    req_analog_label_idx;
+
+    double req_fc_debug_rate;
+    double req_nc_debug_rate;
+    double req_navi_rate;
+    double req_reset_rate;
+    double req_external_control_rate;
+
+    //
+    // Callback functions
+    //
+    void readSerialCallback(const ros::TimerEvent&);
+    void reqVersionCallback(const ros::TimerEvent&);
+    void reqAnalogLabelCallback(const ros::TimerEvent&);
+    void reqNcDebugCallback(const ros::TimerEvent&);
+    void reqFcDebugCallback(const ros::TimerEvent&);
+    void reqNaviCallback(const ros::TimerEvent&);
+    void reqResetCallback(const std_msgs::Empty& msg);
+    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);
+    void getControlCallback(const ros::TimerEvent&);
+    
+    //
+    // Message field extraction functions
+    //
+    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);
+    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);
+    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);
+    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);
+    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);
+    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);
+    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);
+    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);
+    
+    //
+    // Message handling functions
+    //
+    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);
+
+    //
+    // Utility functions
+    //
+    void redirectSerialTo(MikrokopterInterface::Address addr);
+    
+    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);
+    int calculateChecksum(std::vector<unsigned char>& buffer);
+    int calculateChecksum(unsigned char* buffer, int len);
+    bool isValidChecksum(std::vector<unsigned char>& buffer);
+    void updateWithChecksum(unsigned char* buffer, int len);
+    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);
+    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);
+    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);
+    uint8_t connectedToAddress;
+  };
+  
+}
+
+
+
+#endif
diff --git a/mikrokopter/mikrokopter/launch/joystick.launch b/mikrokopter/mikrokopter/launch/joystick.launch
new file mode 100644
index 0000000..bee1307
--- /dev/null
+++ b/mikrokopter/mikrokopter/launch/joystick.launch
@@ -0,0 +1,10 @@
+<launch>
+  <node name="joy_node" pkg="joy" type="joy_node">
+  </node>
+  <node name="joystick_control" pkg="mikrokopter" type="joystick_control">
+    <!-- rate in Hz to publish Control messages -->
+    <param name="publish_rate" value="20" />
+  </node>
+</launch>
+
+
diff --git a/mikrokopter/mikrokopter/launch/mikrokopter.launch b/mikrokopter/mikrokopter/launch/mikrokopter.launch
new file mode 100644
index 0000000..09a9f72
--- /dev/null
+++ b/mikrokopter/mikrokopter/launch/mikrokopter.launch
@@ -0,0 +1,18 @@
+<launch>
+
+  <node name="mikrokopter" pkg="mikrokopter" 	type="mikrokopter">
+    
+    <param name="port" value="/dev/ttyUSB0" />
+    	
+    <param name="req_version_rate" value="0.0" />	
+    <param name="req_version_addr" value="97" />
+    
+    <param name="req_analog_label_rate" value="0.0" />
+    <param name="req_nc_debug_rate" value="0.0" />
+    <param name="req_fc_debug_rate" value="10.0" />
+    <param name="req_navi_rate" value="0.0" />
+    <param name="req_external_control_rate" value="0.0" />
+    
+  </node>
+       
+</launch>
diff --git a/mikrokopter/mikrokopter/launch/onboard.launch b/mikrokopter/mikrokopter/launch/onboard.launch
new file mode 100644
index 0000000..b5a6079
--- /dev/null
+++ b/mikrokopter/mikrokopter/launch/onboard.launch
@@ -0,0 +1,7 @@
+<launch>
+  <include file="$(find mikrokopter)/launch/mikrokopter.launch" />
+  <node name="mk_wrapper" pkg="mikrokopter" type="mk_wrapper">
+    <!-- if no Control message is received in this time in ms, go dead-->
+    <param name="max_quiet_time" value="200" />
+  </node>
+</launch>
diff --git a/mikrokopter/mikrokopter/mainpage.dox b/mikrokopter/mikrokopter/mainpage.dox
new file mode 100644
index 0000000..024b5c5
--- /dev/null
+++ b/mikrokopter/mikrokopter/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b mikrokopter is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
diff --git a/mikrokopter/mikrokopter/manifest.xml b/mikrokopter/mikrokopter/manifest.xml
new file mode 100644
index 0000000..7ffd9b2
--- /dev/null
+++ b/mikrokopter/mikrokopter/manifest.xml
@@ -0,0 +1,18 @@
+<package>
+  <description brief="mikrokopter">
+
+     mikrokopter
+
+  </description>
+  <author>Andrew Chambers/achamber@gmail.com, Justin Haines/jhaines@andrew.cmu.edu, Sebastian Scherer/basti@andrew.cmu.edu</author>
+  <license>GPLv3</license>
+  <review status="unreviewed" notes=""/>
+  <url>https://aeroscoutsim.frc.cs.cmu.edu/redmine/projects/riverine/wiki/Mikrokopter</url>
+  <depend package="std_msgs" />
+  <depend package="sensor_msgs" />
+  <depend package="roscpp" />
+  <rosdep name="libserial" />
+
+</package>
+
+
diff --git a/mikrokopter/mikrokopter/msg/AnalogLabel.msg b/mikrokopter/mikrokopter/msg/AnalogLabel.msg
new file mode 100644
index 0000000..18272e9
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/AnalogLabel.msg
@@ -0,0 +1,5 @@
+# Labels of the analog values in the Debug Data Struct 
+Header header
+uint8 slaveAddr
+uint8 index
+string label
diff --git a/mikrokopter/mikrokopter/msg/AngleOut.msg b/mikrokopter/mikrokopter/msg/AngleOut.msg
new file mode 100644
index 0000000..fd0e93d
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/AngleOut.msg
@@ -0,0 +1,8 @@
+Header header
+
+uint8 slaveAddr
+
+int16[2] angle
+uint8[2] userParameter
+uint8 calcState
+uint8 orientation
diff --git a/mikrokopter/mikrokopter/msg/Control.msg b/mikrokopter/mikrokopter/msg/Control.msg
new file mode 100644
index 0000000..fd05b43
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Control.msg
@@ -0,0 +1,29 @@
+# Feeds the ExternControl struct in the Mikrokopter
+# struct str_ExternControl
+# {
+#  unsigned char Digital[2];
+#  unsigned char RemoteTasten;
+#  signed char   Nick;
+#  signed char   Roll;
+#  signed char   Gier;
+#  unsigned char Gas;
+#  signed char   Hight;
+#  unsigned char free;
+#  unsigned char Frame;
+#  unsigned char Config;
+# };
+
+Header header
+
+uint8[2] digital
+uint8 remoteKey
+int8 pitch
+int8 roll
+int8 yaw
+uint8 thrust
+int8 height
+uint8 free
+uint8 frame
+uint8 config
+
+
diff --git a/mikrokopter/mikrokopter/msg/FcDebug.msg b/mikrokopter/mikrokopter/msg/FcDebug.msg
new file mode 100644
index 0000000..a61cc5f
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/FcDebug.msg
@@ -0,0 +1,42 @@
+Header header
+
+float32 updateRate_hz
+uint8 slaveAddr
+
+uint8[2] status
+
+int32 pitchAngle
+int32 rollAngle
+int32 accPitch
+int32 accRoll
+int32 gyroYaw
+int32 heightValue
+int32 accZ
+int32 gas
+int32 compassValue
+int32 batteryVoltage
+
+int32 receiverLevel
+int32 gyroCompass
+int32 engineFront
+int32 engineBack
+int32 engineLeft
+int32 engineRight
+int32 onesix
+int32 oneseven
+int32 oneeight
+int32 onenine
+
+int32 servo
+int32 hovergas
+int32 current
+int32 capacity_mAh
+int32 heightSetpoint
+int32 twofive
+int32 twosix
+int32 compassSetpoint
+int32 i2c_error
+int32 bl_limit
+
+int32 GPS_Pitch
+int32 GPS_Roll
diff --git a/mikrokopter/mikrokopter/msg/GpsPos.msg b/mikrokopter/mikrokopter/msg/GpsPos.msg
new file mode 100644
index 0000000..3057aa2
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/GpsPos.msg
@@ -0,0 +1,4 @@
+int32 longitude
+int32 latitude
+int32 altitude
+uint8 status
diff --git a/mikrokopter/mikrokopter/msg/GpsPosDev.msg b/mikrokopter/mikrokopter/msg/GpsPosDev.msg
new file mode 100644
index 0000000..05a4403
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/GpsPosDev.msg
@@ -0,0 +1,2 @@
+uint16 distance
+int16 bearing
diff --git a/mikrokopter/mikrokopter/msg/Navi.msg b/mikrokopter/mikrokopter/msg/Navi.msg
new file mode 100644
index 0000000..9b48b9b
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Navi.msg
@@ -0,0 +1,37 @@
+Header header
+
+float32 updateRate_hz
+uint8 slaveAddr
+
+uint8 version
+GpsPos currentPosition
+GpsPos targetPosition
+GpsPosDev targetPositionDeviation
+GpsPos homePosition
+GpsPosDev homePositionDeviation
+uint8 waypointIndex
+uint8 waypointNumber
+uint8 satsInUse
+int16 altimeter
+
+int16 variometer
+uint16 flyingTime
+uint8 uBat
+uint16 groundSpeed
+int16 heading
+int16 compassHeading
+int8 angleNick
+int8 angleRoll
+uint8 rcQuality
+uint8 fcStatusFlags
+
+uint8 ncFlags
+uint8 errorCode
+uint8 operatingRadius
+int16 topSpeed
+uint8 targetHoldTime
+uint8 fcStatusFlags2
+int16 setpointAltitude
+uint8 gas
+uint16 current
+uint16 usedCapacity
diff --git a/mikrokopter/mikrokopter/msg/NcDebug.msg b/mikrokopter/mikrokopter/msg/NcDebug.msg
new file mode 100644
index 0000000..e011a77
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/NcDebug.msg
@@ -0,0 +1,43 @@
+Header header
+
+float32 updateRate_hz
+uint8 slaveAddr
+
+uint8[2] status
+
+int32 angleNick
+int32 angleRoll
+int32 accNick
+int32 accRoll
+int32 operatingRadius
+int32 fcFlags
+int32 ncFlags
+int32 nickServo
+int32 rollServo
+int32 gpsData
+
+int32 compassHeading
+int32 gyroHeading
+int32 spiError
+int32 spiOkay
+int32 i2cError
+int32 i2cOkay
+int32 poiIndex          # Kalman_K
+int32 accSpeedN
+int32 accSpeedE
+int32 speedZ            # GPS ACC
+
+int32 empty20
+int32 nSpeed
+int32 eSpeed
+int32 empty23
+int32 magnetX
+int32 magnetY
+int32 magnetZ
+int32 distanceN
+int32 distanceE
+int32 gpsNick
+
+int32 gpsRoll
+int32 usedSats
+
diff --git a/mikrokopter/mikrokopter/msg/PPM.msg b/mikrokopter/mikrokopter/msg/PPM.msg
new file mode 100644
index 0000000..98fb4be
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/PPM.msg
@@ -0,0 +1,2 @@
+Header header
+int32[27] channels
diff --git a/mikrokopter/mikrokopter/msg/RC.msg b/mikrokopter/mikrokopter/msg/RC.msg
new file mode 100644
index 0000000..eebeafc
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/RC.msg
@@ -0,0 +1,4 @@
+Header header
+int16[8] channels
+bool	 valid_signal
+
diff --git a/mikrokopter/mikrokopter/msg/Status.msg b/mikrokopter/mikrokopter/msg/Status.msg
new file mode 100644
index 0000000..29abee7
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Status.msg
@@ -0,0 +1,7 @@
+Header header
+float64 battery_volt
+bool 	flying
+bool 	motors_on
+bool 	autonomous
+
+
diff --git a/mikrokopter/mikrokopter/msg/Thrust.msg b/mikrokopter/mikrokopter/msg/Thrust.msg
new file mode 100644
index 0000000..a7bcd29
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Thrust.msg
@@ -0,0 +1,2 @@
+Header header
+float64[4] thrust
\ No newline at end of file
diff --git a/mikrokopter/mikrokopter/msg/Velocity2D.msg b/mikrokopter/mikrokopter/msg/Velocity2D.msg
new file mode 100644
index 0000000..1cfde14
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Velocity2D.msg
@@ -0,0 +1,3 @@
+Header header
+float64 forward
+float64 lateral
diff --git a/mikrokopter/mikrokopter/msg/Version.msg b/mikrokopter/mikrokopter/msg/Version.msg
new file mode 100644
index 0000000..d6d5e7a
--- /dev/null
+++ b/mikrokopter/mikrokopter/msg/Version.msg
@@ -0,0 +1,28 @@
+# // bitmask for HardwareError[0]
+# FC_ERROR0_GYRO_NICK 	0x01
+# FC_ERROR0_GYRO_ROLL 	0x02
+# FC_ERROR0_GYRO_YAW 		0x04
+# FC_ERROR0_ACC_NICK 		0x08
+# FC_ERROR0_ACC_ROLL 		0x10
+# FC_ERROR0_ACC_TOP  		0x20
+# FC_ERROR0_PRESSURE		0x40
+# FC_ERROR0_CAREFREE		0x80
+
+# // bitmask for HardwareError[1]
+# FC_ERROR1_I2C   	 	  0x01
+# FC_ERROR1_BL_MISSING 	0x02
+# FC_ERROR1_SPI_RX	 	  0x04
+# FC_ERROR1_PPM	 		    0x08
+# FC_ERROR1_MIXER			  0x10
+# FC_ERROR1_RES1			  0x20
+# FC_ERROR1_RES2			  0x40
+# FC_ERROR1_RES3			  0x80
+
+Header header
+uint8 slaveAddr
+uint8 swMajor
+uint8 swMinor
+uint8 protoMajor
+uint8 protoMinor
+uint8 swPatch
+uint8[5] hardwareError
diff --git a/mikrokopter/mikrokopter/rosdep.yaml b/mikrokopter/mikrokopter/rosdep.yaml
new file mode 100644
index 0000000..d9a51f2
--- /dev/null
+++ b/mikrokopter/mikrokopter/rosdep.yaml
@@ -0,0 +1,3 @@
+libserial:
+  ubuntu: libserial0 libserial-dev
+  debian: libserial-dev
diff --git a/mikrokopter/mikrokopter/src/CoordToPID.cpp b/mikrokopter/mikrokopter/src/CoordToPID.cpp
new file mode 100644
index 0000000..3abeb7a
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/CoordToPID.cpp
@@ -0,0 +1,93 @@
+#include "nav_lib.h"
+#include <geometry_msgs/Point.h>
+#include <math.h>
+#include "pid_control.h"
+#include <time.h>
+
+class CoordToPID
+{
+public:
+  CoordToPID();
+  void mainLoop();
+  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
+
+private:
+  MikrokopterControl control;
+  PID_control pidanglecontrol;
+  PID_control pidvelcontrol;
+  ros::Publisher pub;
+  ros::NodeHandle n;
+  ros::Subscriber sub;
+  int goaldistance;
+};
+
+CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)
+{
+  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
+  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
+      &CoordToPID::pointCallback, this);
+ 
+  //Sets Goal values for the PID	
+  //Goal angle is 0 degrees
+  float p = 0.4;
+  float d = 0.01;
+  float i = 0.4;
+  PID_control pidanglecontrol = PID_control(p,d,i,0);
+  
+  //pdi values for velocity	
+  p = 0.4;
+  d = 0.01;
+  i = 0.4;
+  //Target distance to stay from target
+  goaldistance = 100; //What units this in?
+  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);
+}
+
+void CoordToPID::mainLoop()
+{
+  /*ros::Rate loop_rate(25);
+  while(ros::ok())
+  {
+    ros::spinOnce();
+    control.publish_on(pub);
+    loop_rate.sleep();
+  }*/
+  control.main_loop();
+}
+
+void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)
+{
+  	//call on Priya's PID
+	/* Initializes the PID controller 
+	PID_control(int p_term, int d_term, int i_term, float goal); 
+
+	Initializes the PID controller without goal term 
+	PID_control(int p_term, int d_term, int i_term); 
+
+	Reset the I term error and change the goal value 
+	void change_goal(float goal);
+
+	Given an input, get the PID controlled output 
+	float pid(float input)*/
+
+	// angle going CCW from forward in horizontal plane
+  	float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
+ 	 // should be from [-100, 100]
+	float distance = sqrt(pow(p->x,2)+pow(p->y,2));
+	//Updates pid with current value and gets pid output
+	float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));
+	float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));
+    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);	
+	
+    //sets pid output
+	control.set_yaw(pidangle*100/M_PI);
+	control.velocity_control(pidvelocity,0);
+	//controls.set_thrust() //between [0,1]
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "CoordToPID");
+  CoordToPID ctp = CoordToPID();
+  ctp.mainLoop();
+  return 0;
+}
diff --git a/mikrokopter/mikrokopter/src/externctrl_client.cpp b/mikrokopter/mikrokopter/src/externctrl_client.cpp
new file mode 100644
index 0000000..8169ef7
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/externctrl_client.cpp
@@ -0,0 +1,66 @@
+ /*!
+  *  \file    externctrl_client.cpp
+  *  \brief   A demonstration of the MikrokopterInterface class
+  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
+  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
+  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
+  *  \date    2011 October 25
+  *  \details This program sends commands to a Mikrokopter rotorcraft via ROS
+  */
+
+#include "ros/ros.h"
+#include "mikrokopter/Control.h"
+#include <cstdlib>
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mikrokopter_externctrl_client");
+
+  ros::NodeHandle n;
+  ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
+  
+  mikrokopter::Control req;
+
+  req.digital[0] = 0;
+  req.digital[1] = 0;
+  req.remoteKey = 0;
+  req.pitch = 0;
+  req.roll = 0;
+  req.yaw = 0;
+  req.thrust = 40;
+  req.height = 0;
+  req.free = 0;
+  req.frame = 7;
+  req.config = 1;
+  
+  ros::Rate loop_rate(25);
+  
+  
+  int maxPt = 30;
+  int minPt = -30;
+  int step = 2;
+  int val = 30;
+  
+  while (ros::ok())
+  {
+
+    if (val >= maxPt)
+      step = -1*step;
+    if (val <= minPt)
+      step = -1*step;
+  
+    val += step;
+
+    std::cout << val << std::endl;
+  
+    req.frame++;
+    req.roll = val;
+
+    pub.publish(req);
+
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+
+  return 0;
+}
diff --git a/mikrokopter/mikrokopter/src/interface.cpp b/mikrokopter/mikrokopter/src/interface.cpp
new file mode 100644
index 0000000..6c19daa
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/interface.cpp
@@ -0,0 +1,888 @@
+ /*!
+  *  \file    interface.cpp
+  *  \brief   Implements the MikrokopterInterface class
+  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
+  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
+  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
+  *  \date    2011 October 25
+  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
+  */
+
+#include "mikrokopter/interface.h"
+using namespace CA;
+
+MikrokopterInterface::MikrokopterInterface(ros::NodeHandle nh):
+    n(nh)
+{
+  //
+  // Advertise all of the messages that this node might publish
+  //
+  ppm_data_pub = n.advertise<mikrokopter::PPM>("/mikrokopter/ppm", 100);
+  fc_debug_data_pub = n.advertise<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100);
+  nc_debug_data_pub = n.advertise<mikrokopter::NcDebug>("/mikrokopter/nc_debug", 100);
+  navi_data_pub = n.advertise<mikrokopter::Navi>("/mikrokopter/navi", 100);
+  version_data_pub = n.advertise<mikrokopter::Version>("/mikrokopter/version", 100);
+  label_data_pub = n.advertise<mikrokopter::AnalogLabel>("/mikrokopter/analog_label", 100);
+  set_control_ack_pub = n.advertise<mikrokopter::Control>("/mikrokopter/set_control_ack", 100);  
+  control_data_pub = n.advertise<mikrokopter::Control>("/mikrokopter/extern_control", 100);
+  angleout_data_pub = n.advertise<mikrokopter::AngleOut>("/mikrokopter/angle_out", 100);
+  
+  //
+  // Subscribe to the external control and reset request topics
+  //
+  set_control_sub = n.subscribe("/mikrokopter/req_set_control", 100, &MikrokopterInterface::setControlCallback, this);
+  req_reset_sub = n.subscribe("/mikrokopter/req_reset", 100, &MikrokopterInterface::reqResetCallback, this);
+
+  //
+  // Check to see if we should be sending periodic requests to the MK
+  //
+  if(n.getParam("/mikrokopter/req_version_rate",req_version_rate) && req_version_rate > 0.0)
+  {
+    if(!n.getParam("mikrokopter/req_version_addr",req_version_addr))
+    {
+      ROS_ERROR_STREAM("Must define req_version_addr");
+      ros::shutdown();
+    }
+    req_version_timer = n.createTimer(ros::Duration(1/req_version_rate), &MikrokopterInterface::reqVersionCallback, this);
+  }
+
+  if(n.getParam("/mikrokopter/req_analog_label_rate",req_analog_label_rate) && req_analog_label_rate > 0.0)
+  {
+    req_analog_label_idx = 0;
+    req_analog_label_timer = n.createTimer(ros::Duration(1/req_analog_label_rate), &MikrokopterInterface::reqAnalogLabelCallback, this);
+  }
+  
+  if(n.getParam("/mikrokopter/req_nc_debug_rate",req_nc_debug_rate) && req_nc_debug_rate > 0.0)
+  {
+    req_nc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNcDebugCallback, this);
+  }
+
+  if(n.getParam("/mikrokopter/req_fc_debug_rate",req_fc_debug_rate) && req_fc_debug_rate > 0.0)
+  {
+    req_fc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqFcDebugCallback, this);
+  }
+
+  if(n.getParam("/mikrokopter/req_navi_rate",req_navi_rate) && req_navi_rate > 0.0)
+  {
+    req_navi_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNaviCallback, this);
+  }
+
+  if(n.getParam("/mikrokopter/req_external_control_rate",req_external_control_rate) && req_external_control_rate > 0.0)
+  {
+    req_external_control_timer = n.createTimer(ros::Duration(1/req_external_control_rate), &MikrokopterInterface::getControlCallback, this);
+  }
+
+  //
+  // Set up the serial port and callback
+  //
+  readSerialTimer = n.createTimer(ros::Duration(0.02), &MikrokopterInterface::readSerialCallback, this);
+  
+  // Open serial port
+  std::string portname;
+  if(!n.getParam("/mikrokopter/port",portname))
+  {
+    ROS_ERROR_STREAM("Port not defined");
+    ros::shutdown();
+  }
+	
+  fd.Open(portname,std::ios::in|std::ios::out);
+	
+  if (!fd.good())
+  {
+    ROS_ERROR_STREAM( "Error : unable to open " << portname);
+    ros::shutdown();
+  }
+  else
+  {
+    ROS_INFO_STREAM( "Opening port: " << portname );
+  }
+  
+  fd.SetBaudRate(SerialStreamBuf::BAUD_57600);
+  fd.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
+  fd.SetNumOfStopBits(1);
+  fd.SetParity(SerialStreamBuf::PARITY_NONE);
+  fd.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);//NONE
+  
+  //
+  // Create a lookup table mapping from message ID to message length
+  //
+  msg_len_table['A'] = 30;
+  msg_len_table['B'] = 10;
+  msg_len_table['V'] = 22;
+  msg_len_table['D'] = 94;
+  msg_len_table['G'] = 22;
+  msg_len_table['k'] = 18;
+  msg_len_table['P'] = 78;
+  msg_len_table['O'] = 118;
+
+  redirectSerialTo(NcAddress);
+}
+
+MikrokopterInterface::~MikrokopterInterface()
+{
+  fd.Close();
+}
+
+void MikrokopterInterface::redirectSerialTo(MikrokopterInterface::Address addr)
+{
+    //
+    // Nothing to do if we are already connected to the right board
+    //
+    if(connectedToAddress == addr)
+    {
+      return;
+    }
+
+    if (addr == FcAddress)
+    {
+      unsigned char commandbuffer[512];
+      int cmdlen = 0;
+      unsigned char board[]={0};  // uart selector for FC
+      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
+      fd.write((const char*)commandbuffer,cmdlen);  
+      
+    } else if (addr == Mk3MagAddress)
+    {
+      unsigned char commandbuffer[512];
+      int cmdlen = 0;
+      unsigned char board[]={1};  // uart selector for MK3MAG
+      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
+      fd.write((const char*)commandbuffer,cmdlen); 
+    }
+    /*else if (addr == MkGpsAddress)
+    {
+      unsigned char commandbuffer[512];
+      int cmdlen = 0;
+      unsigned char board[]={2};  // uart selector for MKGPS
+      createCommand(commandbuffer,cmdlen,'u',NC_ADDRESS,board,1);
+      fd.write((const char*)commandbuffer,cmdlen); 
+    }*/
+    else // By default connect to NaviCtrl Bd
+    {
+      unsigned char commandbuffer[] = {0x1B,0x1B,0x55,0xAA,0x00};  // Magic packet defined by Mikrokopter serial protocol
+      int cmdlen = 5;
+      fd.write((const char*)commandbuffer,cmdlen);
+    }
+  
+  connectedToAddress = addr;
+  
+  return;
+
+}
+
+void MikrokopterInterface::reqResetCallback(const std_msgs::Empty& msg)
+{
+  int cmdlen;
+  unsigned char commandbuffer[512];  
+  createCommand(commandbuffer,cmdlen,'R',BlankAddress,NULL,0);
+  fd.write((const char*)commandbuffer,cmdlen);
+  ROS_DEBUG_STREAM("Sent Reset Request to MK");
+}
+
+void MikrokopterInterface::reqVersionCallback(const ros::TimerEvent&)
+{
+  int cmdlen;
+  unsigned char commandbuffer[512];  
+  createCommand(commandbuffer,cmdlen,'v',req_version_addr,NULL,0);
+  fd.write((const char*)commandbuffer,cmdlen);
+  ROS_DEBUG_STREAM("Sent Version Request to MK");
+}
+
+void MikrokopterInterface::reqAnalogLabelCallback(const ros::TimerEvent&)
+{
+  unsigned char index[1];
+  index[0] = req_analog_label_idx;
+  
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'a',BlankAddress,index,1);
+  fd.write((const char*)commandbuffer,cmdlen);
+
+  req_analog_label_idx++;
+  req_analog_label_idx %= 32;
+
+  ROS_DEBUG_STREAM("Sent Analog Label Request to MK");
+}
+
+void MikrokopterInterface::reqNcDebugCallback(const ros::TimerEvent&)
+{
+  redirectSerialTo(NcAddress);
+    
+  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
+  // Subsciption needs to be renewed every 4s.
+  unsigned char rate[1];
+  rate[0] = (unsigned char) 100.0/(req_nc_debug_rate);
+  
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'d',NcAddress,rate,1);
+  fd.write((const char*)commandbuffer,cmdlen);
+
+  ROS_DEBUG_STREAM("Sent NC Debug Request to MK");
+}
+
+void MikrokopterInterface::reqFcDebugCallback(const ros::TimerEvent&)
+{
+  redirectSerialTo(FcAddress);
+      
+  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
+  // Subsciption needs to be renewed every 4s.
+  unsigned char rate[1];
+  rate[0] = (unsigned char) 100.0/(req_fc_debug_rate);
+    
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'d',FcAddress,rate,1);
+  fd.write((const char*)commandbuffer,cmdlen);
+
+  ROS_DEBUG_STREAM("Sent FC Debug Request to MK");
+}
+
+void MikrokopterInterface::reqNaviCallback(const ros::TimerEvent&)
+{
+  redirectSerialTo(NcAddress);
+    
+  // Navi Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds.
+  unsigned char rate[1];
+  rate[0] = (unsigned char) 100.0/(req_navi_rate);
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'o',NcAddress,rate,1);
+  fd.write((const char*)commandbuffer,cmdlen);
+
+  ROS_DEBUG_STREAM("Sent Navi Request to MK");
+  
+}
+
+void MikrokopterInterface::setControlCallback(const mikrokopter::Control::ConstPtr& msg)
+{
+  redirectSerialTo(FcAddress);
+
+  unsigned char dataBytes[11];
+	    
+  dataBytes[0] = msg->digital[0];
+  dataBytes[1] = msg->digital[1];
+  dataBytes[2] = msg->remoteKey;
+  dataBytes[3] = msg->pitch;
+  dataBytes[4] = msg->roll;
+  dataBytes[5] = msg->yaw;
+  dataBytes[6] = msg->thrust;
+  dataBytes[7] = msg->height;
+  dataBytes[8] = msg->free;
+  dataBytes[9] = msg->frame;
+  dataBytes[10] = msg->config;
+  
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'b',FcAddress,dataBytes,11);
+  fd.write((const char*)commandbuffer,cmdlen);
+  ROS_DEBUG_STREAM("Sent Set Control Request to MK");
+}
+
+void MikrokopterInterface::getControlCallback(const ros::TimerEvent&)
+{
+  int cmdlen;
+  unsigned char commandbuffer[512];
+  createCommand(commandbuffer,cmdlen,'g',FcAddress,NULL,0);
+  fd.write((const char*)commandbuffer,cmdlen);
+  ROS_DEBUG_STREAM("Sent Get Control Request to MK");
+}
+
+void MikrokopterInterface::extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  message.swMajor     = outbuffer[0];
+  message.swMinor     = outbuffer[1];
+  message.protoMajor  = outbuffer[2];
+  message.protoMinor  = outbuffer[3];
+  message.swPatch     = outbuffer[4];
+  
+  for (int i = 0; i < 5; i++)
+  {
+    message.hardwareError[i] = outbuffer[i+5];
+  }
+  
+}
+
+void MikrokopterInterface::extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  message.digital[0]    = outbuffer[0];
+  message.digital[1]    = outbuffer[1];
+  message.remoteKey     = outbuffer[2];
+  message.pitch         = (signed int)outbuffer[3];
+  message.roll          = (signed int)outbuffer[4];
+  message.yaw           = (signed int)outbuffer[5];  
+  message.thrust        = outbuffer[6];
+  message.height        = (signed int)outbuffer[7];
+  message.free          = outbuffer[8];
+  message.frame         = outbuffer[9];
+  message.config        = outbuffer[10];
+  
+}
+
+void MikrokopterInterface::extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  msg.angle[0] = (int16_t) (outbuffer[1] << 8 | outbuffer[0]);
+  msg.angle[1] = (int16_t) (outbuffer[3] << 8 | outbuffer[2]);
+  msg.userParameter[0] = (uint8_t) outbuffer[4];
+  msg.userParameter[1] = (uint8_t) outbuffer[5];
+  msg.calcState = (uint8_t) outbuffer[6];
+  msg.orientation = (uint8_t) outbuffer[7];  
+}
+
+void MikrokopterInterface::extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  message.index     = outbuffer[0];
+ 
+  for (int i = 0; i < 16; i++)
+    {
+      message.label += outbuffer[i+1];
+    }
+}
+
+
+void MikrokopterInterface::extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  for (int i=0; i<27; i++)
+  {
+    message.channels[i] = (signed int) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
+  }
+}
+
+
+void MikrokopterInterface::extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  debug_data.status[0] = outbuffer[0];
+  debug_data.status[1] = outbuffer[1];
+  
+  int val[32];
+  for (int i=0; i<32; i++)
+  {
+    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
+  }
+
+  debug_data.pitchAngle                       = val[0];
+  debug_data.rollAngle                        = val[1];
+  debug_data.accPitch                         = val[2];
+  debug_data.accRoll                          = val[3];
+  debug_data.gyroYaw                          = val[4];
+  debug_data.heightValue                      = val[5];
+  debug_data.accZ                             = val[6];
+  debug_data.gas                              = val[7];
+  debug_data.compassValue                     = val[8];
+  debug_data.batteryVoltage                   = val[9];
+	
+  debug_data.receiverLevel                    = val[10];
+  debug_data.gyroCompass                      = val[11];
+  debug_data.engineFront                      = val[12];
+  debug_data.engineBack                       = val[13];
+  debug_data.engineLeft                       = val[14];
+  debug_data.engineRight                      = val[15];
+  debug_data.onesix                           = val[16];
+  debug_data.oneseven    			                = val[17];
+  debug_data.oneeight                         = val[18];
+  debug_data.onenine                          = val[19];
+	
+  debug_data.servo                            = val[20];
+  debug_data.hovergas                         = val[21];
+  debug_data.current                          = val[22];
+  debug_data.capacity_mAh                     = val[23];
+  debug_data.heightSetpoint                   = val[24];
+  debug_data.twofive                          = val[25];
+  debug_data.twosix                           = val[26];
+  debug_data.compassSetpoint                  = val[27];
+  debug_data.i2c_error                        = val[28];
+  debug_data.bl_limit	                        = val[29];
+
+  debug_data.GPS_Pitch                        = val[30];
+  debug_data.GPS_Roll                         = val[31];
+ 
+}
+
+void MikrokopterInterface::extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+  
+  debug_data.status[0] = outbuffer[0];
+  debug_data.status[1] = outbuffer[1];
+  
+  int val[32];
+  for (int i=0; i<32; i++){
+    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
+  }
+
+  debug_data.angleNick = val[0];
+  debug_data.angleRoll = val[1];
+  debug_data.accNick = val[2];
+  debug_data.accRoll = val[3];
+  debug_data.operatingRadius = val[4];
+  debug_data.fcFlags = val[5];
+  debug_data.ncFlags = val[6];
+  debug_data.nickServo = val[7];
+  debug_data.rollServo = val[8];
+  debug_data.gpsData = val[9];
+	
+  debug_data.compassHeading = val[10];
+  debug_data.gyroHeading = val[11];
+  debug_data.spiError = val[12];
+  debug_data.spiOkay = val[13];
+  debug_data.i2cError = val[14];
+  debug_data.i2cOkay = val[15];
+  debug_data.poiIndex = val[16];
+  debug_data.accSpeedN = val[17];
+  debug_data.accSpeedE = val[18];
+  debug_data.speedZ = val[19];
+	
+  debug_data.empty20 = val[20];
+  debug_data.nSpeed = val[21];
+  debug_data.eSpeed = val[22];
+  debug_data.empty23 = val[23];
+  debug_data.magnetX = val[24];
+  debug_data.magnetY = val[25];
+  debug_data.magnetZ = val[26];
+  debug_data.distanceN = val[27];
+  debug_data.distanceE = val[28];
+  debug_data.gpsNick = val[29];
+
+  debug_data.gpsRoll = val[30];
+  debug_data.usedSats = val[31];
+ 
+}
+
+void MikrokopterInterface::extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg)
+{
+  unsigned char outbuffer[512];
+  decode64(buffer,0,outbuffer);
+
+  msg.version                 = (uint8_t)outbuffer[0];
+  msg.currentPosition         = processGpsPos(outbuffer+1);
+  msg.targetPosition          = processGpsPos(outbuffer+14);
+  msg.targetPositionDeviation = processGpsPosDev(outbuffer+27);
+  msg.homePosition            = processGpsPos(outbuffer+31);
+  msg.homePositionDeviation   = processGpsPosDev(outbuffer+44);
+  msg.waypointIndex           = (uint8_t) outbuffer[48];
+  msg.waypointNumber          = (uint8_t) outbuffer[49];
+  msg.satsInUse               = (uint8_t) outbuffer[50];
+  msg.altimeter               = (int16_t) (outbuffer[52] << 8 | outbuffer[51]);
+  msg.variometer              = (int16_t) (outbuffer[54] << 8 | outbuffer[53]);
+  msg.flyingTime              = (uint16_t) (outbuffer[56] << 8 | outbuffer[55]);
+  msg.uBat                    = (uint8_t) outbuffer[57];
+  msg.groundSpeed             = (uint16_t) (outbuffer[59] << 8 | outbuffer[58]);
+  msg.heading                 = (int16_t) (outbuffer[61] << 8 | outbuffer[60]);
+  msg.compassHeading          = (int16_t) (outbuffer[63] << 8 | outbuffer[62]);
+  msg.angleNick               = (int8_t) outbuffer[64];
+  msg.angleRoll               = (int8_t) outbuffer[65];
+  msg.rcQuality               = (uint8_t) outbuffer[66];
+  msg.fcStatusFlags           = (uint8_t) outbuffer[67];
+  msg.ncFlags                 = (uint8_t) outbuffer[68];
+  msg.errorCode               = (uint8_t) outbuffer[69];
+  msg.operatingRadius         = (uint8_t) outbuffer[70];
+  msg.topSpeed                = (int16_t) (outbuffer[72] << 8 | outbuffer[71]);
+  msg.targetHoldTime          = (uint8_t) outbuffer[73];
+  msg.fcStatusFlags2          = (uint8_t) outbuffer[74];
+  msg.setpointAltitude        = (int16_t) (outbuffer[76] << 8 | outbuffer[75]);
+  msg.gas                     = (uint8_t) outbuffer[77];
+  msg.current                 = (uint16_t) (outbuffer[79] << 8 | outbuffer[78]);
+  msg.usedCapacity            = (uint16_t) (outbuffer[81] << 8 | outbuffer[80]);
+}
+
+void MikrokopterInterface::process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    uint8_t slaveAddr =  buffer[1] - 'a';
+    mikrokopter::NcDebug debug_data;
+    debug_data.header.frame_id="mikrokopter";
+    debug_data.header.stamp=stamp;
+    debug_data.slaveAddr=slaveAddr - 'a';
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_nc_debug_message(buffer, debug_data);
+    nc_debug_data_pub.publish(debug_data);
+
+    ROS_DEBUG_STREAM( "Published NC debug package");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for nc debug msg");
+  }
+}
+void MikrokopterInterface::process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    uint8_t slaveAddr =  buffer[1] - 'a';
+    mikrokopter::FcDebug debug_data;
+    debug_data.header.frame_id="mikrokopter";
+    debug_data.header.stamp=stamp;
+    debug_data.slaveAddr=slaveAddr - 'a';
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_fc_debug_message(buffer, debug_data);
+
+    fc_debug_data_pub.publish(debug_data);
+
+    ROS_DEBUG_STREAM( "Published FC debug package");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for fc debug msg");
+  }
+}
+
+void MikrokopterInterface::process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( buffer[1] == NcAddress && isValidChecksum(buffer) ) {
+
+    mikrokopter::Navi naviMsg;
+    naviMsg.header.frame_id="mikrokopter";
+    naviMsg.header.stamp = stamp;
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_navi_message(buffer, naviMsg);
+    navi_data_pub.publish(naviMsg);
+
+    ROS_DEBUG_STREAM( "Published navi data package");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for navi data msg");
+  }
+}
+
+void MikrokopterInterface::process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( buffer[1] == FcAddress && isValidChecksum(buffer) ) {
+
+    mikrokopter::PPM ppm_data;
+    ppm_data.header.frame_id="mikrokopter";
+    ppm_data.header.stamp = stamp;
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_ppm_message(buffer, ppm_data);
+    ppm_data_pub.publish(ppm_data);
+
+    ROS_DEBUG_STREAM( "Published ppm package");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for ppm msg");
+  }
+}
+void MikrokopterInterface::process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    mikrokopter::AnalogLabel label_data;
+    label_data.header.frame_id="mikrokopter";
+    label_data.header.stamp=stamp;
+    label_data.slaveAddr   = buffer[1];
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_label_message(buffer, label_data);
+    label_data_pub.publish(label_data);
+
+    ROS_DEBUG_STREAM( "Published Analog Label message");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for analog label msg");
+  }
+}
+void MikrokopterInterface::process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    mikrokopter::Control msg;
+    msg.header.frame_id="mikrokopter";
+    msg.header.stamp=stamp;
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_control_message(buffer, msg);
+    control_data_pub.publish(msg);
+
+    ROS_DEBUG_STREAM( "Published ExternControl message");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for ExternControl msg");
+  }
+}
+void MikrokopterInterface::process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    mikrokopter::Version version_data;
+    version_data.header.frame_id="mikrokopter";
+    version_data.header.stamp=stamp;
+    version_data.slaveAddr   = buffer[1] - 'a';
+
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    extract_version_message(buffer, version_data);
+    version_data_pub.publish(version_data);
+
+    ROS_DEBUG_STREAM( "Published Version Info message");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for version info msg");
+  }
+}
+void MikrokopterInterface::process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( buffer[1] == Mk3MagAddress && isValidChecksum(buffer) )
+  {
+     mikrokopter::AngleOut msg;
+     msg.header.frame_id = "mikrokopter";
+     msg.header.stamp = stamp;
+     msg.slaveAddr = buffer[1] - 'a';
+
+     buffer.erase(buffer.begin(),buffer.begin()+3);
+     extract_angleout_message(buffer, msg);
+     angleout_data_pub.publish(msg);
+
+     ROS_DEBUG_STREAM("Published Angle Out");
+
+   } else {
+     ROS_WARN_STREAM("Checksum failed for heading msg");
+   }
+}
+
+void MikrokopterInterface::process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp)
+{
+  if ( isValidChecksum(buffer) )
+  {
+    mikrokopter::Control msg;
+    msg.header.frame_id="mikrokopter";
+    msg.header.stamp=stamp;
+
+    unsigned char outbuffer[512];
+    buffer.erase(buffer.begin(),buffer.begin()+3);
+    decode64(buffer,0,outbuffer);
+    msg.frame     = outbuffer[0];
+
+    set_control_ack_pub.publish(msg);
+
+    ROS_DEBUG_STREAM( "Published ExternControl ACK");
+
+  } else {
+    ROS_WARN_STREAM( " Checksum failed for ExternControl ACK");
+  }
+}
+
+mikrokopter::GpsPos MikrokopterInterface::processGpsPos(unsigned char* buffer)
+{
+  mikrokopter::GpsPos gps;
+  gps.longitude = (int32_t)(buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]);
+  gps.latitude = (int32_t)(buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]);
+  gps.altitude = (int32_t)(buffer[11] << 24 | buffer[10] << 16 | buffer[9] << 8 | buffer[8]);
+  gps.status = buffer[12];
+  
+  return gps;
+}
+
+mikrokopter::GpsPosDev MikrokopterInterface::processGpsPosDev(unsigned char* buffer)
+{
+  mikrokopter::GpsPosDev dev;
+  dev.distance = (uint32_t)(buffer[1] << 8 | buffer[0]);
+  dev.bearing = (int32_t)(buffer[3] << 8 | buffer[2]);
+  
+  return dev;
+}
+
+void MikrokopterInterface::readSerialCallback(const ros::TimerEvent&)
+{
+  char c;
+  static ros::Time nextStamp, stamp;
+
+  while (fd.rdbuf()->in_avail()>0 && ros::ok()) // While there is data in the serial receive buffer
+  {
+    fd.get(c);
+    buffer.push_back(c);
+
+    if (c == '#')
+    {
+      stamp = nextStamp;
+      nextStamp = ros::Time::now();
+      buffer.erase(buffer.begin(),buffer.end()-1);
+    }
+
+    if ( buffer[0] == '#' && buffer.size() >= 2)
+    {
+      if( buffer.size() == msg_len_table[buffer[2]])
+      {
+        switch (buffer[2])
+        {
+          case 'A':
+            process_label_message(buffer,stamp);
+            break;
+
+          case 'B':
+            process_external_control_ack_message(buffer,stamp);
+            break;
+
+          case 'H':
+            ROS_WARN_STREAM("Unimplemented message \"H\"");
+            break;
+
+          case 'L':
+            ROS_WARN_STREAM("Unimplemented message \"L\"");
+            break;
+
+          case 'V':
+            process_version_message(buffer,stamp);
+            break;
+
+          case 'D':
+            {
+              uint8_t slaveAddr =  buffer[1];
+              if(slaveAddr == FcAddress)
+              {
+                process_fc_debug_message(buffer,stamp);
+              }
+              else if(slaveAddr == NcAddress)
+              {
+                process_nc_debug_message(buffer,stamp);
+              }
+            }
+            break;
+
+          case 'G':
+            process_control_message(buffer,stamp);
+            break;
+
+          case 'k':
+            process_angleout_message(buffer,stamp);
+            break;
+
+          case 'P':
+            process_ppm_message(buffer,stamp);
+            break;
+
+          case 'O':
+            process_navi_message(buffer,stamp);
+            break;
+
+          default:
+            ROS_WARN_STREAM("Unknown message type: "<<buffer[2]);
+            break;
+
+        } // end switch (buffer[2])
+      } // end if( buffer.size() == msg_len_map[buffer[2]])
+    } // end if ( buffer[0] == '#')
+  } // end while (fd.rdbuf()->in_avail()>0 && ros::ok())
+}
+
+
+void MikrokopterInterface::decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr) {
+  int ptrIn=offset; 
+  int a,b,c,d,x,y,z;
+  int ptr=0;
+  int len = in_arr.size();
+  while(len!=0)
+    {
+      a = in_arr[ptrIn++] - '=';
+      b = in_arr[ptrIn++] - '=';
+      c = in_arr[ptrIn++] - '=';
+      d = in_arr[ptrIn++] - '=';
+ 
+      x = (a << 2) | (b >> 4);
+      y = ((b & 0x0f) << 4) | (c >> 2);
+      z = ((c & 0x03) << 6) | d;
+
+      if((len--)!=0) out_arr[ptr++] = x; else break;
+      if((len--)!=0) out_arr[ptr++] = y; else break;
+      if((len--)!=0) out_arr[ptr++] = z; else break;
+    }
+}
+
+int MikrokopterInterface::calculateChecksum(std::vector<unsigned char>& buffer)
+{
+  int chk = 0;
+  unsigned int l=0;
+  for (l=0; l<buffer.size()-3; l++){
+    chk += buffer[l];
+  }
+  chk %= 4096;
+  return chk;
+}
+
+int MikrokopterInterface::calculateChecksum(unsigned char* buffer, int len)
+{
+  int chk = 0;
+  int l=0;
+  for (l=0; l<len-3; l++){
+    chk += buffer[l];
+  }
+  chk %= 4096;
+  return chk;
+}
+  
+bool MikrokopterInterface::isValidChecksum(std::vector<unsigned char>& buffer)
+{
+  int chk = calculateChecksum(buffer);
+  
+  // Verify the Checksum
+  int l=buffer.size()-3;
+  return (buffer[l++] == ('=' + chk / 64) && 
+	  buffer[l++] == ('=' + chk % 64) && 
+	  buffer[l++] == '\r');
+}
+
+void MikrokopterInterface::updateWithChecksum(unsigned char* buffer, int len)
+{
+  int chks = calculateChecksum(buffer, len);
+
+  buffer[len-3] = ('=' + chks / 64);
+  buffer[len-2] = ('=' + chks % 64);
+}
+
+void MikrokopterInterface::createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len)
+{
+  msglen = 0;
+  commandbuffer[msglen++] = '#';
+  commandbuffer[msglen++] = address;
+  commandbuffer[msglen++] = cmd;
+  unsigned char a,b,c;
+  a=0;b=0;c=0;
+  int ptr=0;
+  while(len)
+    {
+      if(len>0)
+	{
+	  a = data[ptr++];
+	  len--;
+	}
+      else a = 0;
+      if(len>0)
+	{
+	  b = data[ptr++];
+	  len--;
+	}
+      else b = 0;
+      if(len>0)
+	{
+	  c = data[ptr++];
+	  len--;
+	}
+      else c = 0;
+      commandbuffer[msglen++] = '=' + (a >> 2);
+      commandbuffer[msglen++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
+      commandbuffer[msglen++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
+      commandbuffer[msglen++] = '=' + ( c & 0x3f);
+    }
+  commandbuffer[msglen++] ='x';
+  commandbuffer[msglen++] ='x';
+  commandbuffer[msglen++] ='\r';
+  updateWithChecksum(commandbuffer,msglen);
+}
diff --git a/mikrokopter/mikrokopter/src/joystick_control.cpp b/mikrokopter/mikrokopter/src/joystick_control.cpp
new file mode 100644
index 0000000..2401e0e
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/joystick_control.cpp
@@ -0,0 +1,56 @@
+/**
+ * @brief A node responsible for reading joystick messages from ros
+ *        joystick_drivers to control the MikroKopter
+ *
+ * @author CMU Quadrotor Project
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Joy.h>
+#include <stdio.h>
+
+#include "nav_lib.h"
+
+class JoystickControl
+{
+    public:
+        JoystickControl();
+        void joyLoop();
+        void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
+
+    private:
+        MikrokopterControl control;
+        ros::Publisher pub;
+        ros::NodeHandle n;
+        ros::Subscriber sub;
+};
+
+JoystickControl::JoystickControl()
+{
+  sub = n.subscribe<sensor_msgs::Joy>("joy", 100,
+      &JoystickControl::joyCallback, this);
+}
+
+void JoystickControl::joyLoop()
+{
+    control.main_loop();
+}
+
+void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
+{
+    control.velocity_control(joy->axes[1], -joy->axes[0]);
+    control.set_yaw(-joy->axes[2]);
+    control.set_thrust((joy->axes[3]+1)/2); ///Adjust thrust to be between [0, 1] instead of [-1, 1]
+    return;
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "joystick_control");
+    JoystickControl joy_control;
+
+    // This commands loops infinitely
+    joy_control.joyLoop();
+
+    return 0;
+}
diff --git a/mikrokopter/mikrokopter/src/keyboard_control.cpp b/mikrokopter/mikrokopter/src/keyboard_control.cpp
new file mode 100644
index 0000000..a0d19ce
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/keyboard_control.cpp
@@ -0,0 +1,125 @@
+/**
+ * @brief A node responsible for keyboard commands which will be sent to
+ *        the MikroKopter
+ *
+ * @author CMU Quadrotor Project
+ * @author Alex Zirbel
+ */
+
+#include <ros/ros.h>
+#include <signal.h>
+#include <termios.h>
+#include <stdio.h>
+#include <ncurses.h>
+
+#include "nav_lib.h"
+
+#define KEYCODE_R 0x43 
+#define KEYCODE_L 0x44
+#define KEYCODE_U 0x41
+#define KEYCODE_D 0x42
+#define KEYCODE_Q 0x71
+
+class KeyboardControl
+{
+    public:
+        KeyboardControl();
+        void keyLoop();
+
+    private:
+        ros::NodeHandle n;
+};
+
+KeyboardControl::KeyboardControl()
+{
+}
+
+void KeyboardControl::keyLoop()
+{
+    
+    ros::NodeHandle n;
+    ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
+
+    MikrokopterControl control;
+
+    ros::Rate loop_rate(25);
+    
+    char c;
+
+    // get the console in raw mode
+    initscr();
+    timeout(0);
+    noecho();
+    cbreak();
+
+    puts("Reading from keyboard");
+    puts("---------------------------");
+    puts("Use arrow keys to command the quadrotor.");
+
+    while(ros::ok())
+    {
+        // get the next event from the keyboard  
+        c = getch();
+
+        //ROS_INFO("key press: 0x%02X\n", c);
+        //ROS_INFO("key press: %c\n", c);
+
+        switch(c)
+        {
+            case KEYCODE_L:
+            case 'a':
+                ROS_INFO("A");
+                control.left();
+                break;
+            case KEYCODE_R:
+            case 'd':
+                ROS_INFO("D");
+                control.right();
+                break;
+            case KEYCODE_U:
+            case 'w':
+                ROS_INFO("W");
+                control.forward();
+                break;
+            case KEYCODE_D:
+            case 's':
+                ROS_INFO("B");
+                control.backward();
+                break;
+            case ' ':
+                ROS_INFO("space");
+                control.level();
+                break;
+            case 'r':
+                ROS_INFO("UP");
+                //set_throttle(&pub, );
+                break;
+            case 'f':
+                ROS_INFO("DOWN");
+                //set_throttle(&pub);
+                break;
+            default:
+                control.level();
+        }
+
+        control.publish_on(pub);
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    
+    endwin();
+    return;
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "keyboard_control");
+    KeyboardControl teleop_turtle;
+
+    // This commands loops infinitely
+    teleop_turtle.keyLoop();
+
+    return(0);
+}
+
+
diff --git a/mikrokopter/mikrokopter/src/mikrokopter_node.cpp b/mikrokopter/mikrokopter/src/mikrokopter_node.cpp
new file mode 100644
index 0000000..ef6ae9c
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/mikrokopter_node.cpp
@@ -0,0 +1,23 @@
+ /*!
+  *  \file    mikrokopter_node.cpp
+  *  \brief   Instantiates the MikrokopterInterface class
+  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
+  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
+  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
+  *  \date    2011 October 25
+  *  \details This program is a ROS wrapper for the MikrokopterInterface
+  */
+
+#include <ros/ros.h>
+#include <mikrokopter/interface.h>
+
+using namespace CA;
+
+int main(int argc, char **argv)
+{
+   ros::init(argc, argv, "mikrokopter");
+   ros::NodeHandle n;
+ 
+   MikrokopterInterface interface(n);
+   ros::spin();
+}
diff --git a/mikrokopter/mikrokopter/src/mk_wrapper.cpp b/mikrokopter/mikrokopter/src/mk_wrapper.cpp
new file mode 100644
index 0000000..0bb7f3c
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/mk_wrapper.cpp
@@ -0,0 +1,78 @@
+#include <ros/ros.h>
+#include <mikrokopter/Control.h>
+
+class MKWrapper
+{
+public:
+    MKWrapper();
+    void main_loop();
+    void control_callback(const mikrokopter::Control& control);
+    
+private:
+    ros::Publisher pub;
+    ros::Subscriber sub;
+    ros::NodeHandle nh;
+    ros::Duration max_quiet_time;
+    ros::Time last_time_recv;
+    bool started, dead;
+};
+
+MKWrapper::MKWrapper()
+{
+    pub = nh.advertise<mikrokopter::Control>("/mikrokopter/req_set_control",
+        100); 
+    sub = nh.subscribe("/mk_wrapper/control", 100, &MKWrapper::control_callback,
+        this);
+    int max_quiet_time_ms;
+    ros::param::param<int>("~max_quiet_time", max_quiet_time_ms, 1000);
+    ROS_INFO("Max quiet time: %d", max_quiet_time_ms);
+    max_quiet_time.fromSec(max_quiet_time_ms / 1000.);
+    started = false;
+    dead = false;
+}
+
+void MKWrapper::main_loop()
+{
+    mikrokopter::Control control_dead;
+    control_dead.pitch = 0;
+    control_dead.roll = 0;
+    control_dead.yaw = 0;
+    control_dead.thrust = 0;
+    
+    ros::Rate loop_rate(20);
+    while (ros::ok())
+    {
+        if (started)
+        {
+            if (dead)
+            {
+                pub.publish(control_dead);
+            }
+            else if (ros::Time::now() - last_time_recv > max_quiet_time)
+            {
+                dead = true;
+                ROS_ERROR("Lost connection: mk_wrapper must be restarted");
+            }
+        }
+        loop_rate.sleep();
+        ros::spinOnce();
+    }
+}
+
+void MKWrapper::control_callback(const mikrokopter::Control& control)
+{
+    started = true;
+    if (!dead)
+    {
+        pub.publish(control);
+        last_time_recv = ros::Time::now();
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "mk_wrapper");
+    MKWrapper wrapper;
+    wrapper.main_loop();
+    return 0;
+}
diff --git a/mikrokopter/mikrokopter/src/nav_lib.cpp b/mikrokopter/mikrokopter/src/nav_lib.cpp
new file mode 100644
index 0000000..9f1030f
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/nav_lib.cpp
@@ -0,0 +1,96 @@
+#include "nav_lib.h"
+
+MikrokopterControl::MikrokopterControl()
+{
+    control.digital[0] = 0;
+    control.digital[1] = 0;
+    control.remoteKey = 0;
+    control.pitch = 0;
+    control.roll = 0;
+    control.yaw = 0;
+    control.thrust = 40;
+    control.height = 0;
+    control.free = 0;
+    control.frame = 7;
+    control.config = 1;
+}
+
+void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed)
+{
+    if(forward_speed > 1) forward_speed = 1;
+    if(forward_speed < -1) forward_speed = -1;
+    if(lateral_speed > 1) lateral_speed = 1;
+    if(lateral_speed < -1) lateral_speed = -1; 
+    int k = 50; // TODO set scaling constant and check signs
+    control.roll = k*(forward_speed - lateral_speed);
+    control.pitch = k*(forward_speed + lateral_speed);
+}
+
+void MikrokopterControl::forward()
+{
+    control.roll = 15;
+    control.pitch = 15;
+}
+
+void MikrokopterControl::backward()
+{
+    control.roll = -15;
+    control.pitch = -15;
+}
+
+void MikrokopterControl::left()
+{
+    control.roll = 15;
+    control.pitch = -15;
+}
+
+void MikrokopterControl::right()
+{
+    control.roll = -15;
+    control.pitch = 15;
+}
+
+void MikrokopterControl::level()
+{
+    control.roll = 0;
+    control.pitch = 0;
+}
+
+void MikrokopterControl::set_thrust(float thrust)
+{
+    if (thrust < 0) thrust = 0;
+    if (thrust > 1) thrust = 1;
+    int k = 255;
+    control.thrust = k*thrust;
+}
+
+void MikrokopterControl::set_yaw(float yaw)
+{
+    if(yaw>1) yaw = 1;
+    if(yaw<-1) yaw = -1;
+    int k = 50;;
+    control.yaw = k*yaw;
+}
+
+void MikrokopterControl::publish_on(ros::Publisher& pub)
+{
+    pub.publish(control);
+}
+
+void MikrokopterControl::main_loop()
+{
+    ros::Publisher pub = nh.advertise< ::mikrokopter::Control>(
+        "/mk_wrapper/control", 200);
+    
+    int publish_rate;
+    ros::param::param<int>("~publish_rate", publish_rate, 20);
+    ros::Rate loop_rate(publish_rate);
+    
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        pub.publish(control);
+        loop_rate.sleep();
+    }
+    
+}
diff --git a/mikrokopter/mikrokopter/src/nav_lib.h b/mikrokopter/mikrokopter/src/nav_lib.h
new file mode 100644
index 0000000..5e02bee
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/nav_lib.h
@@ -0,0 +1,20 @@
+#include "ros/ros.h"
+#include "mikrokopter/Control.h"
+
+class MikrokopterControl {
+public:
+    MikrokopterControl();
+    void velocity_control(float forward_speed, float lateral_speed);
+    void forward();
+    void backward();
+    void left();
+    void right();
+    void level();
+    void set_thrust(float thrust);
+    void set_yaw(float yaw);
+    void publish_on(ros::Publisher& pub);
+    void main_loop();
+private:
+    mikrokopter::Control control;
+    ros::NodeHandle nh;
+};
diff --git a/mikrokopter/mikrokopter/src/node_control.cpp b/mikrokopter/mikrokopter/src/node_control.cpp
new file mode 100644
index 0000000..6145c1c
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/node_control.cpp
@@ -0,0 +1,51 @@
+#include "ros/ros.h"
+#include "std_msgs/Float64.h"
+#include "mikrokopter/Velocity2D.h"
+#include "nav_lib.h"
+
+class NodeControl
+{
+public:
+  NodeControl();
+  void main_loop() {control.main_loop();}
+  void cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n);
+  void cb_yaw(const std_msgs::Float64::ConstPtr& n);
+  void cb_thrust(const std_msgs::Float64::ConstPtr& n);
+
+private:
+  MikrokopterControl control;
+  ros::NodeHandle n;
+  ros::Subscriber sub_velocity;
+  ros::Subscriber sub_yaw;
+  ros::Subscriber sub_thrust;
+};
+
+NodeControl::NodeControl()
+{
+  sub_velocity = n.subscribe<mikrokopter::Velocity2D>("/mikrokopter/velocity",
+      100, &NodeControl::cb_velocity, this);
+  sub_yaw = n.subscribe<std_msgs::Float64>("/mikrokopter/yaw", 100,
+      &NodeControl::cb_yaw, this);
+  sub_thrust= n.subscribe<std_msgs::Float64>("/mikrokopter/thrust", 100,
+      &NodeControl::cb_thrust, this);
+}
+
+void NodeControl::cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n)
+{
+  control.velocity_control(n->forward, n->lateral);
+}
+void NodeControl::cb_yaw(const std_msgs::Float64::ConstPtr& n)
+{
+  control.set_yaw(n->data);
+}
+void NodeControl::cb_thrust(const std_msgs::Float64::ConstPtr& n)
+{
+  control.set_thrust(n->data);
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "node_control");
+  NodeControl control = NodeControl();
+  control.main_loop();
+  return 0;
+}
diff --git a/mikrokopter/mikrokopter/src/turn_to_target.cpp b/mikrokopter/mikrokopter/src/turn_to_target.cpp
new file mode 100644
index 0000000..5714638
--- /dev/null
+++ b/mikrokopter/mikrokopter/src/turn_to_target.cpp
@@ -0,0 +1,51 @@
+#include "nav_lib.h"
+#include <geometry_msgs/Point.h>
+#include <math.h>
+
+class TurnToTarget
+{
+public:
+  TurnToTarget();
+  void mainLoop();
+  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
+
+private:
+  MikrokopterControl control;
+  ros::Publisher pub;
+  ros::NodeHandle n;
+  ros::Subscriber sub;
+};
+
+TurnToTarget::TurnToTarget()
+{
+  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
+  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
+      &TurnToTarget::pointCallback, this);
+}
+
+void TurnToTarget::mainLoop()
+{
+  /*ros::Rate loop_rate(25);
+  while(ros::ok())
+  {
+    ros::spinOnce();
+    control.publish_on(pub);
+    loop_rate.sleep();
+  }*/
+  control.main_loop();
+}
+
+void TurnToTarget::pointCallback(const geometry_msgs::Point::ConstPtr& p)
+{
+  // angle going CCW from forward in horizontal plane
+  float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
+  // should be from [-100, 100]
+  control.set_yaw(angle*100/M_PI);
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "turn_to_target");
+  TurnToTarget ttt = TurnToTarget();
+  ttt.mainLoop();
+  return 0;
+}
diff --git a/mikrokopter/msg/AnalogLabel.msg b/mikrokopter/msg/AnalogLabel.msg
deleted file mode 100644
index 18272e9..0000000
--- a/mikrokopter/msg/AnalogLabel.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-# Labels of the analog values in the Debug Data Struct 
-Header header
-uint8 slaveAddr
-uint8 index
-string label
diff --git a/mikrokopter/msg/AngleOut.msg b/mikrokopter/msg/AngleOut.msg
deleted file mode 100644
index fd0e93d..0000000
--- a/mikrokopter/msg/AngleOut.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-Header header
-
-uint8 slaveAddr
-
-int16[2] angle
-uint8[2] userParameter
-uint8 calcState
-uint8 orientation
diff --git a/mikrokopter/msg/Control.msg b/mikrokopter/msg/Control.msg
deleted file mode 100644
index fd05b43..0000000
--- a/mikrokopter/msg/Control.msg
+++ /dev/null
@@ -1,29 +0,0 @@
-# Feeds the ExternControl struct in the Mikrokopter
-# struct str_ExternControl
-# {
-#  unsigned char Digital[2];
-#  unsigned char RemoteTasten;
-#  signed char   Nick;
-#  signed char   Roll;
-#  signed char   Gier;
-#  unsigned char Gas;
-#  signed char   Hight;
-#  unsigned char free;
-#  unsigned char Frame;
-#  unsigned char Config;
-# };
-
-Header header
-
-uint8[2] digital
-uint8 remoteKey
-int8 pitch
-int8 roll
-int8 yaw
-uint8 thrust
-int8 height
-uint8 free
-uint8 frame
-uint8 config
-
-
diff --git a/mikrokopter/msg/FcDebug.msg b/mikrokopter/msg/FcDebug.msg
deleted file mode 100644
index a61cc5f..0000000
--- a/mikrokopter/msg/FcDebug.msg
+++ /dev/null
@@ -1,42 +0,0 @@
-Header header
-
-float32 updateRate_hz
-uint8 slaveAddr
-
-uint8[2] status
-
-int32 pitchAngle
-int32 rollAngle
-int32 accPitch
-int32 accRoll
-int32 gyroYaw
-int32 heightValue
-int32 accZ
-int32 gas
-int32 compassValue
-int32 batteryVoltage
-
-int32 receiverLevel
-int32 gyroCompass
-int32 engineFront
-int32 engineBack
-int32 engineLeft
-int32 engineRight
-int32 onesix
-int32 oneseven
-int32 oneeight
-int32 onenine
-
-int32 servo
-int32 hovergas
-int32 current
-int32 capacity_mAh
-int32 heightSetpoint
-int32 twofive
-int32 twosix
-int32 compassSetpoint
-int32 i2c_error
-int32 bl_limit
-
-int32 GPS_Pitch
-int32 GPS_Roll
diff --git a/mikrokopter/msg/GpsPos.msg b/mikrokopter/msg/GpsPos.msg
deleted file mode 100644
index 3057aa2..0000000
--- a/mikrokopter/msg/GpsPos.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-int32 longitude
-int32 latitude
-int32 altitude
-uint8 status
diff --git a/mikrokopter/msg/GpsPosDev.msg b/mikrokopter/msg/GpsPosDev.msg
deleted file mode 100644
index 05a4403..0000000
--- a/mikrokopter/msg/GpsPosDev.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint16 distance
-int16 bearing
diff --git a/mikrokopter/msg/Navi.msg b/mikrokopter/msg/Navi.msg
deleted file mode 100644
index 9b48b9b..0000000
--- a/mikrokopter/msg/Navi.msg
+++ /dev/null
@@ -1,37 +0,0 @@
-Header header
-
-float32 updateRate_hz
-uint8 slaveAddr
-
-uint8 version
-GpsPos currentPosition
-GpsPos targetPosition
-GpsPosDev targetPositionDeviation
-GpsPos homePosition
-GpsPosDev homePositionDeviation
-uint8 waypointIndex
-uint8 waypointNumber
-uint8 satsInUse
-int16 altimeter
-
-int16 variometer
-uint16 flyingTime
-uint8 uBat
-uint16 groundSpeed
-int16 heading
-int16 compassHeading
-int8 angleNick
-int8 angleRoll
-uint8 rcQuality
-uint8 fcStatusFlags
-
-uint8 ncFlags
-uint8 errorCode
-uint8 operatingRadius
-int16 topSpeed
-uint8 targetHoldTime
-uint8 fcStatusFlags2
-int16 setpointAltitude
-uint8 gas
-uint16 current
-uint16 usedCapacity
diff --git a/mikrokopter/msg/NcDebug.msg b/mikrokopter/msg/NcDebug.msg
deleted file mode 100644
index e011a77..0000000
--- a/mikrokopter/msg/NcDebug.msg
+++ /dev/null
@@ -1,43 +0,0 @@
-Header header
-
-float32 updateRate_hz
-uint8 slaveAddr
-
-uint8[2] status
-
-int32 angleNick
-int32 angleRoll
-int32 accNick
-int32 accRoll
-int32 operatingRadius
-int32 fcFlags
-int32 ncFlags
-int32 nickServo
-int32 rollServo
-int32 gpsData
-
-int32 compassHeading
-int32 gyroHeading
-int32 spiError
-int32 spiOkay
-int32 i2cError
-int32 i2cOkay
-int32 poiIndex          # Kalman_K
-int32 accSpeedN
-int32 accSpeedE
-int32 speedZ            # GPS ACC
-
-int32 empty20
-int32 nSpeed
-int32 eSpeed
-int32 empty23
-int32 magnetX
-int32 magnetY
-int32 magnetZ
-int32 distanceN
-int32 distanceE
-int32 gpsNick
-
-int32 gpsRoll
-int32 usedSats
-
diff --git a/mikrokopter/msg/PPM.msg b/mikrokopter/msg/PPM.msg
deleted file mode 100644
index 98fb4be..0000000
--- a/mikrokopter/msg/PPM.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-int32[27] channels
diff --git a/mikrokopter/msg/RC.msg b/mikrokopter/msg/RC.msg
deleted file mode 100644
index eebeafc..0000000
--- a/mikrokopter/msg/RC.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-Header header
-int16[8] channels
-bool	 valid_signal
-
diff --git a/mikrokopter/msg/Status.msg b/mikrokopter/msg/Status.msg
deleted file mode 100644
index 29abee7..0000000
--- a/mikrokopter/msg/Status.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-Header header
-float64 battery_volt
-bool 	flying
-bool 	motors_on
-bool 	autonomous
-
-
diff --git a/mikrokopter/msg/Thrust.msg b/mikrokopter/msg/Thrust.msg
deleted file mode 100644
index a7bcd29..0000000
--- a/mikrokopter/msg/Thrust.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-float64[4] thrust
\ No newline at end of file
diff --git a/mikrokopter/msg/Velocity2D.msg b/mikrokopter/msg/Velocity2D.msg
deleted file mode 100644
index 1cfde14..0000000
--- a/mikrokopter/msg/Velocity2D.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-float64 forward
-float64 lateral
diff --git a/mikrokopter/msg/Version.msg b/mikrokopter/msg/Version.msg
deleted file mode 100644
index d6d5e7a..0000000
--- a/mikrokopter/msg/Version.msg
+++ /dev/null
@@ -1,28 +0,0 @@
-# // bitmask for HardwareError[0]
-# FC_ERROR0_GYRO_NICK 	0x01
-# FC_ERROR0_GYRO_ROLL 	0x02
-# FC_ERROR0_GYRO_YAW 		0x04
-# FC_ERROR0_ACC_NICK 		0x08
-# FC_ERROR0_ACC_ROLL 		0x10
-# FC_ERROR0_ACC_TOP  		0x20
-# FC_ERROR0_PRESSURE		0x40
-# FC_ERROR0_CAREFREE		0x80
-
-# // bitmask for HardwareError[1]
-# FC_ERROR1_I2C   	 	  0x01
-# FC_ERROR1_BL_MISSING 	0x02
-# FC_ERROR1_SPI_RX	 	  0x04
-# FC_ERROR1_PPM	 		    0x08
-# FC_ERROR1_MIXER			  0x10
-# FC_ERROR1_RES1			  0x20
-# FC_ERROR1_RES2			  0x40
-# FC_ERROR1_RES3			  0x80
-
-Header header
-uint8 slaveAddr
-uint8 swMajor
-uint8 swMinor
-uint8 protoMajor
-uint8 protoMinor
-uint8 swPatch
-uint8[5] hardwareError
diff --git a/mikrokopter/rosdep.yaml b/mikrokopter/rosdep.yaml
deleted file mode 100644
index d9a51f2..0000000
--- a/mikrokopter/rosdep.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-libserial:
-  ubuntu: libserial0 libserial-dev
-  debian: libserial-dev
diff --git a/mikrokopter/src/CoordToPID.cpp b/mikrokopter/src/CoordToPID.cpp
deleted file mode 100644
index c0e0f3a..0000000
--- a/mikrokopter/src/CoordToPID.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-#include "nav_lib.h"
-#include <geometry_msgs/Point.h>
-#include <math.h>
-#include "../../control/pid_control.h"
-#include <time.h>
-
-class CoordToPID
-{
-public:
-  CoordToPID();
-  void mainLoop();
-  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
-
-private:
-  MikrokopterControl control;
-  PID_control pidanglecontrol;
-  PID_control pidvelcontrol;
-  ros::Publisher pub;
-  ros::NodeHandle n;
-  ros::Subscriber sub;
-  int goaldistance;
-};
-
-CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)
-{
-  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
-  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
-      &CoordToPID::pointCallback, this);
- 
-  //Sets Goal values for the PID	
-  //Goal angle is 0 degrees
-  float p = 0.4;
-  float d = 0.01;
-  float i = 0.4;
-  PID_control pidanglecontrol = PID_control(p,d,i,0);
-  
-  //pdi values for velocity	
-  p = 0.4;
-  d = 0.01;
-  i = 0.4;
-  //Target distance to stay from target
-  goaldistance = 100; //What units this in?
-  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);
-}
-
-void CoordToPID::mainLoop()
-{
-  /*ros::Rate loop_rate(25);
-  while(ros::ok())
-  {
-    ros::spinOnce();
-    control.publish_on(pub);
-    loop_rate.sleep();
-  }*/
-  control.main_loop();
-}
-
-void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)
-{
-  	//call on Priya's PID
-	/* Initializes the PID controller 
-	PID_control(int p_term, int d_term, int i_term, float goal); 
-
-	Initializes the PID controller without goal term 
-	PID_control(int p_term, int d_term, int i_term); 
-
-	Reset the I term error and change the goal value 
-	void change_goal(float goal);
-
-	Given an input, get the PID controlled output 
-	float pid(float input)*/
-
-	// angle going CCW from forward in horizontal plane
-  	float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
- 	 // should be from [-100, 100]
-	float distance = sqrt(pow(p->x,2)+pow(p->y,2));
-	//Updates pid with current value and gets pid output
-	float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));
-	float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));
-    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);	
-	
-    //sets pid output
-	control.set_yaw(pidangle*100/M_PI);
-	control.velocity_control(pidvelocity,0);
-	//controls.set_thrust() //between [0,1]
-}
-
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "CoordToPID");
-  CoordToPID ctp = CoordToPID();
-  ctp.mainLoop();
-  return 0;
-}
diff --git a/mikrokopter/src/externctrl_client.cpp b/mikrokopter/src/externctrl_client.cpp
deleted file mode 100644
index 8169ef7..0000000
--- a/mikrokopter/src/externctrl_client.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
- /*!
-  *  \file    externctrl_client.cpp
-  *  \brief   A demonstration of the MikrokopterInterface class
-  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
-  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
-  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
-  *  \date    2011 October 25
-  *  \details This program sends commands to a Mikrokopter rotorcraft via ROS
-  */
-
-#include "ros/ros.h"
-#include "mikrokopter/Control.h"
-#include <cstdlib>
-
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "mikrokopter_externctrl_client");
-
-  ros::NodeHandle n;
-  ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
-  
-  mikrokopter::Control req;
-
-  req.digital[0] = 0;
-  req.digital[1] = 0;
-  req.remoteKey = 0;
-  req.pitch = 0;
-  req.roll = 0;
-  req.yaw = 0;
-  req.thrust = 40;
-  req.height = 0;
-  req.free = 0;
-  req.frame = 7;
-  req.config = 1;
-  
-  ros::Rate loop_rate(25);
-  
-  
-  int maxPt = 30;
-  int minPt = -30;
-  int step = 2;
-  int val = 30;
-  
-  while (ros::ok())
-  {
-
-    if (val >= maxPt)
-      step = -1*step;
-    if (val <= minPt)
-      step = -1*step;
-  
-    val += step;
-
-    std::cout << val << std::endl;
-  
-    req.frame++;
-    req.roll = val;
-
-    pub.publish(req);
-
-    ros::spinOnce();
-    loop_rate.sleep();
-  }
-
-  return 0;
-}
diff --git a/mikrokopter/src/interface.cpp b/mikrokopter/src/interface.cpp
deleted file mode 100644
index 6c19daa..0000000
--- a/mikrokopter/src/interface.cpp
+++ /dev/null
@@ -1,888 +0,0 @@
- /*!
-  *  \file    interface.cpp
-  *  \brief   Implements the MikrokopterInterface class
-  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
-  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
-  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
-  *  \date    2011 October 25
-  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
-  */
-
-#include "mikrokopter/interface.h"
-using namespace CA;
-
-MikrokopterInterface::MikrokopterInterface(ros::NodeHandle nh):
-    n(nh)
-{
-  //
-  // Advertise all of the messages that this node might publish
-  //
-  ppm_data_pub = n.advertise<mikrokopter::PPM>("/mikrokopter/ppm", 100);
-  fc_debug_data_pub = n.advertise<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100);
-  nc_debug_data_pub = n.advertise<mikrokopter::NcDebug>("/mikrokopter/nc_debug", 100);
-  navi_data_pub = n.advertise<mikrokopter::Navi>("/mikrokopter/navi", 100);
-  version_data_pub = n.advertise<mikrokopter::Version>("/mikrokopter/version", 100);
-  label_data_pub = n.advertise<mikrokopter::AnalogLabel>("/mikrokopter/analog_label", 100);
-  set_control_ack_pub = n.advertise<mikrokopter::Control>("/mikrokopter/set_control_ack", 100);  
-  control_data_pub = n.advertise<mikrokopter::Control>("/mikrokopter/extern_control", 100);
-  angleout_data_pub = n.advertise<mikrokopter::AngleOut>("/mikrokopter/angle_out", 100);
-  
-  //
-  // Subscribe to the external control and reset request topics
-  //
-  set_control_sub = n.subscribe("/mikrokopter/req_set_control", 100, &MikrokopterInterface::setControlCallback, this);
-  req_reset_sub = n.subscribe("/mikrokopter/req_reset", 100, &MikrokopterInterface::reqResetCallback, this);
-
-  //
-  // Check to see if we should be sending periodic requests to the MK
-  //
-  if(n.getParam("/mikrokopter/req_version_rate",req_version_rate) && req_version_rate > 0.0)
-  {
-    if(!n.getParam("mikrokopter/req_version_addr",req_version_addr))
-    {
-      ROS_ERROR_STREAM("Must define req_version_addr");
-      ros::shutdown();
-    }
-    req_version_timer = n.createTimer(ros::Duration(1/req_version_rate), &MikrokopterInterface::reqVersionCallback, this);
-  }
-
-  if(n.getParam("/mikrokopter/req_analog_label_rate",req_analog_label_rate) && req_analog_label_rate > 0.0)
-  {
-    req_analog_label_idx = 0;
-    req_analog_label_timer = n.createTimer(ros::Duration(1/req_analog_label_rate), &MikrokopterInterface::reqAnalogLabelCallback, this);
-  }
-  
-  if(n.getParam("/mikrokopter/req_nc_debug_rate",req_nc_debug_rate) && req_nc_debug_rate > 0.0)
-  {
-    req_nc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNcDebugCallback, this);
-  }
-
-  if(n.getParam("/mikrokopter/req_fc_debug_rate",req_fc_debug_rate) && req_fc_debug_rate > 0.0)
-  {
-    req_fc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqFcDebugCallback, this);
-  }
-
-  if(n.getParam("/mikrokopter/req_navi_rate",req_navi_rate) && req_navi_rate > 0.0)
-  {
-    req_navi_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNaviCallback, this);
-  }
-
-  if(n.getParam("/mikrokopter/req_external_control_rate",req_external_control_rate) && req_external_control_rate > 0.0)
-  {
-    req_external_control_timer = n.createTimer(ros::Duration(1/req_external_control_rate), &MikrokopterInterface::getControlCallback, this);
-  }
-
-  //
-  // Set up the serial port and callback
-  //
-  readSerialTimer = n.createTimer(ros::Duration(0.02), &MikrokopterInterface::readSerialCallback, this);
-  
-  // Open serial port
-  std::string portname;
-  if(!n.getParam("/mikrokopter/port",portname))
-  {
-    ROS_ERROR_STREAM("Port not defined");
-    ros::shutdown();
-  }
-	
-  fd.Open(portname,std::ios::in|std::ios::out);
-	
-  if (!fd.good())
-  {
-    ROS_ERROR_STREAM( "Error : unable to open " << portname);
-    ros::shutdown();
-  }
-  else
-  {
-    ROS_INFO_STREAM( "Opening port: " << portname );
-  }
-  
-  fd.SetBaudRate(SerialStreamBuf::BAUD_57600);
-  fd.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
-  fd.SetNumOfStopBits(1);
-  fd.SetParity(SerialStreamBuf::PARITY_NONE);
-  fd.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);//NONE
-  
-  //
-  // Create a lookup table mapping from message ID to message length
-  //
-  msg_len_table['A'] = 30;
-  msg_len_table['B'] = 10;
-  msg_len_table['V'] = 22;
-  msg_len_table['D'] = 94;
-  msg_len_table['G'] = 22;
-  msg_len_table['k'] = 18;
-  msg_len_table['P'] = 78;
-  msg_len_table['O'] = 118;
-
-  redirectSerialTo(NcAddress);
-}
-
-MikrokopterInterface::~MikrokopterInterface()
-{
-  fd.Close();
-}
-
-void MikrokopterInterface::redirectSerialTo(MikrokopterInterface::Address addr)
-{
-    //
-    // Nothing to do if we are already connected to the right board
-    //
-    if(connectedToAddress == addr)
-    {
-      return;
-    }
-
-    if (addr == FcAddress)
-    {
-      unsigned char commandbuffer[512];
-      int cmdlen = 0;
-      unsigned char board[]={0};  // uart selector for FC
-      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
-      fd.write((const char*)commandbuffer,cmdlen);  
-      
-    } else if (addr == Mk3MagAddress)
-    {
-      unsigned char commandbuffer[512];
-      int cmdlen = 0;
-      unsigned char board[]={1};  // uart selector for MK3MAG
-      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
-      fd.write((const char*)commandbuffer,cmdlen); 
-    }
-    /*else if (addr == MkGpsAddress)
-    {
-      unsigned char commandbuffer[512];
-      int cmdlen = 0;
-      unsigned char board[]={2};  // uart selector for MKGPS
-      createCommand(commandbuffer,cmdlen,'u',NC_ADDRESS,board,1);
-      fd.write((const char*)commandbuffer,cmdlen); 
-    }*/
-    else // By default connect to NaviCtrl Bd
-    {
-      unsigned char commandbuffer[] = {0x1B,0x1B,0x55,0xAA,0x00};  // Magic packet defined by Mikrokopter serial protocol
-      int cmdlen = 5;
-      fd.write((const char*)commandbuffer,cmdlen);
-    }
-  
-  connectedToAddress = addr;
-  
-  return;
-
-}
-
-void MikrokopterInterface::reqResetCallback(const std_msgs::Empty& msg)
-{
-  int cmdlen;
-  unsigned char commandbuffer[512];  
-  createCommand(commandbuffer,cmdlen,'R',BlankAddress,NULL,0);
-  fd.write((const char*)commandbuffer,cmdlen);
-  ROS_DEBUG_STREAM("Sent Reset Request to MK");
-}
-
-void MikrokopterInterface::reqVersionCallback(const ros::TimerEvent&)
-{
-  int cmdlen;
-  unsigned char commandbuffer[512];  
-  createCommand(commandbuffer,cmdlen,'v',req_version_addr,NULL,0);
-  fd.write((const char*)commandbuffer,cmdlen);
-  ROS_DEBUG_STREAM("Sent Version Request to MK");
-}
-
-void MikrokopterInterface::reqAnalogLabelCallback(const ros::TimerEvent&)
-{
-  unsigned char index[1];
-  index[0] = req_analog_label_idx;
-  
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'a',BlankAddress,index,1);
-  fd.write((const char*)commandbuffer,cmdlen);
-
-  req_analog_label_idx++;
-  req_analog_label_idx %= 32;
-
-  ROS_DEBUG_STREAM("Sent Analog Label Request to MK");
-}
-
-void MikrokopterInterface::reqNcDebugCallback(const ros::TimerEvent&)
-{
-  redirectSerialTo(NcAddress);
-    
-  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
-  // Subsciption needs to be renewed every 4s.
-  unsigned char rate[1];
-  rate[0] = (unsigned char) 100.0/(req_nc_debug_rate);
-  
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'d',NcAddress,rate,1);
-  fd.write((const char*)commandbuffer,cmdlen);
-
-  ROS_DEBUG_STREAM("Sent NC Debug Request to MK");
-}
-
-void MikrokopterInterface::reqFcDebugCallback(const ros::TimerEvent&)
-{
-  redirectSerialTo(FcAddress);
-      
-  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
-  // Subsciption needs to be renewed every 4s.
-  unsigned char rate[1];
-  rate[0] = (unsigned char) 100.0/(req_fc_debug_rate);
-    
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'d',FcAddress,rate,1);
-  fd.write((const char*)commandbuffer,cmdlen);
-
-  ROS_DEBUG_STREAM("Sent FC Debug Request to MK");
-}
-
-void MikrokopterInterface::reqNaviCallback(const ros::TimerEvent&)
-{
-  redirectSerialTo(NcAddress);
-    
-  // Navi Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds.
-  unsigned char rate[1];
-  rate[0] = (unsigned char) 100.0/(req_navi_rate);
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'o',NcAddress,rate,1);
-  fd.write((const char*)commandbuffer,cmdlen);
-
-  ROS_DEBUG_STREAM("Sent Navi Request to MK");
-  
-}
-
-void MikrokopterInterface::setControlCallback(const mikrokopter::Control::ConstPtr& msg)
-{
-  redirectSerialTo(FcAddress);
-
-  unsigned char dataBytes[11];
-	    
-  dataBytes[0] = msg->digital[0];
-  dataBytes[1] = msg->digital[1];
-  dataBytes[2] = msg->remoteKey;
-  dataBytes[3] = msg->pitch;
-  dataBytes[4] = msg->roll;
-  dataBytes[5] = msg->yaw;
-  dataBytes[6] = msg->thrust;
-  dataBytes[7] = msg->height;
-  dataBytes[8] = msg->free;
-  dataBytes[9] = msg->frame;
-  dataBytes[10] = msg->config;
-  
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'b',FcAddress,dataBytes,11);
-  fd.write((const char*)commandbuffer,cmdlen);
-  ROS_DEBUG_STREAM("Sent Set Control Request to MK");
-}
-
-void MikrokopterInterface::getControlCallback(const ros::TimerEvent&)
-{
-  int cmdlen;
-  unsigned char commandbuffer[512];
-  createCommand(commandbuffer,cmdlen,'g',FcAddress,NULL,0);
-  fd.write((const char*)commandbuffer,cmdlen);
-  ROS_DEBUG_STREAM("Sent Get Control Request to MK");
-}
-
-void MikrokopterInterface::extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  message.swMajor     = outbuffer[0];
-  message.swMinor     = outbuffer[1];
-  message.protoMajor  = outbuffer[2];
-  message.protoMinor  = outbuffer[3];
-  message.swPatch     = outbuffer[4];
-  
-  for (int i = 0; i < 5; i++)
-  {
-    message.hardwareError[i] = outbuffer[i+5];
-  }
-  
-}
-
-void MikrokopterInterface::extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  message.digital[0]    = outbuffer[0];
-  message.digital[1]    = outbuffer[1];
-  message.remoteKey     = outbuffer[2];
-  message.pitch         = (signed int)outbuffer[3];
-  message.roll          = (signed int)outbuffer[4];
-  message.yaw           = (signed int)outbuffer[5];  
-  message.thrust        = outbuffer[6];
-  message.height        = (signed int)outbuffer[7];
-  message.free          = outbuffer[8];
-  message.frame         = outbuffer[9];
-  message.config        = outbuffer[10];
-  
-}
-
-void MikrokopterInterface::extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  msg.angle[0] = (int16_t) (outbuffer[1] << 8 | outbuffer[0]);
-  msg.angle[1] = (int16_t) (outbuffer[3] << 8 | outbuffer[2]);
-  msg.userParameter[0] = (uint8_t) outbuffer[4];
-  msg.userParameter[1] = (uint8_t) outbuffer[5];
-  msg.calcState = (uint8_t) outbuffer[6];
-  msg.orientation = (uint8_t) outbuffer[7];  
-}
-
-void MikrokopterInterface::extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  message.index     = outbuffer[0];
- 
-  for (int i = 0; i < 16; i++)
-    {
-      message.label += outbuffer[i+1];
-    }
-}
-
-
-void MikrokopterInterface::extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  for (int i=0; i<27; i++)
-  {
-    message.channels[i] = (signed int) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
-  }
-}
-
-
-void MikrokopterInterface::extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  debug_data.status[0] = outbuffer[0];
-  debug_data.status[1] = outbuffer[1];
-  
-  int val[32];
-  for (int i=0; i<32; i++)
-  {
-    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
-  }
-
-  debug_data.pitchAngle                       = val[0];
-  debug_data.rollAngle                        = val[1];
-  debug_data.accPitch                         = val[2];
-  debug_data.accRoll                          = val[3];
-  debug_data.gyroYaw                          = val[4];
-  debug_data.heightValue                      = val[5];
-  debug_data.accZ                             = val[6];
-  debug_data.gas                              = val[7];
-  debug_data.compassValue                     = val[8];
-  debug_data.batteryVoltage                   = val[9];
-	
-  debug_data.receiverLevel                    = val[10];
-  debug_data.gyroCompass                      = val[11];
-  debug_data.engineFront                      = val[12];
-  debug_data.engineBack                       = val[13];
-  debug_data.engineLeft                       = val[14];
-  debug_data.engineRight                      = val[15];
-  debug_data.onesix                           = val[16];
-  debug_data.oneseven    			                = val[17];
-  debug_data.oneeight                         = val[18];
-  debug_data.onenine                          = val[19];
-	
-  debug_data.servo                            = val[20];
-  debug_data.hovergas                         = val[21];
-  debug_data.current                          = val[22];
-  debug_data.capacity_mAh                     = val[23];
-  debug_data.heightSetpoint                   = val[24];
-  debug_data.twofive                          = val[25];
-  debug_data.twosix                           = val[26];
-  debug_data.compassSetpoint                  = val[27];
-  debug_data.i2c_error                        = val[28];
-  debug_data.bl_limit	                        = val[29];
-
-  debug_data.GPS_Pitch                        = val[30];
-  debug_data.GPS_Roll                         = val[31];
- 
-}
-
-void MikrokopterInterface::extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-  
-  debug_data.status[0] = outbuffer[0];
-  debug_data.status[1] = outbuffer[1];
-  
-  int val[32];
-  for (int i=0; i<32; i++){
-    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
-  }
-
-  debug_data.angleNick = val[0];
-  debug_data.angleRoll = val[1];
-  debug_data.accNick = val[2];
-  debug_data.accRoll = val[3];
-  debug_data.operatingRadius = val[4];
-  debug_data.fcFlags = val[5];
-  debug_data.ncFlags = val[6];
-  debug_data.nickServo = val[7];
-  debug_data.rollServo = val[8];
-  debug_data.gpsData = val[9];
-	
-  debug_data.compassHeading = val[10];
-  debug_data.gyroHeading = val[11];
-  debug_data.spiError = val[12];
-  debug_data.spiOkay = val[13];
-  debug_data.i2cError = val[14];
-  debug_data.i2cOkay = val[15];
-  debug_data.poiIndex = val[16];
-  debug_data.accSpeedN = val[17];
-  debug_data.accSpeedE = val[18];
-  debug_data.speedZ = val[19];
-	
-  debug_data.empty20 = val[20];
-  debug_data.nSpeed = val[21];
-  debug_data.eSpeed = val[22];
-  debug_data.empty23 = val[23];
-  debug_data.magnetX = val[24];
-  debug_data.magnetY = val[25];
-  debug_data.magnetZ = val[26];
-  debug_data.distanceN = val[27];
-  debug_data.distanceE = val[28];
-  debug_data.gpsNick = val[29];
-
-  debug_data.gpsRoll = val[30];
-  debug_data.usedSats = val[31];
- 
-}
-
-void MikrokopterInterface::extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg)
-{
-  unsigned char outbuffer[512];
-  decode64(buffer,0,outbuffer);
-
-  msg.version                 = (uint8_t)outbuffer[0];
-  msg.currentPosition         = processGpsPos(outbuffer+1);
-  msg.targetPosition          = processGpsPos(outbuffer+14);
-  msg.targetPositionDeviation = processGpsPosDev(outbuffer+27);
-  msg.homePosition            = processGpsPos(outbuffer+31);
-  msg.homePositionDeviation   = processGpsPosDev(outbuffer+44);
-  msg.waypointIndex           = (uint8_t) outbuffer[48];
-  msg.waypointNumber          = (uint8_t) outbuffer[49];
-  msg.satsInUse               = (uint8_t) outbuffer[50];
-  msg.altimeter               = (int16_t) (outbuffer[52] << 8 | outbuffer[51]);
-  msg.variometer              = (int16_t) (outbuffer[54] << 8 | outbuffer[53]);
-  msg.flyingTime              = (uint16_t) (outbuffer[56] << 8 | outbuffer[55]);
-  msg.uBat                    = (uint8_t) outbuffer[57];
-  msg.groundSpeed             = (uint16_t) (outbuffer[59] << 8 | outbuffer[58]);
-  msg.heading                 = (int16_t) (outbuffer[61] << 8 | outbuffer[60]);
-  msg.compassHeading          = (int16_t) (outbuffer[63] << 8 | outbuffer[62]);
-  msg.angleNick               = (int8_t) outbuffer[64];
-  msg.angleRoll               = (int8_t) outbuffer[65];
-  msg.rcQuality               = (uint8_t) outbuffer[66];
-  msg.fcStatusFlags           = (uint8_t) outbuffer[67];
-  msg.ncFlags                 = (uint8_t) outbuffer[68];
-  msg.errorCode               = (uint8_t) outbuffer[69];
-  msg.operatingRadius         = (uint8_t) outbuffer[70];
-  msg.topSpeed                = (int16_t) (outbuffer[72] << 8 | outbuffer[71]);
-  msg.targetHoldTime          = (uint8_t) outbuffer[73];
-  msg.fcStatusFlags2          = (uint8_t) outbuffer[74];
-  msg.setpointAltitude        = (int16_t) (outbuffer[76] << 8 | outbuffer[75]);
-  msg.gas                     = (uint8_t) outbuffer[77];
-  msg.current                 = (uint16_t) (outbuffer[79] << 8 | outbuffer[78]);
-  msg.usedCapacity            = (uint16_t) (outbuffer[81] << 8 | outbuffer[80]);
-}
-
-void MikrokopterInterface::process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    uint8_t slaveAddr =  buffer[1] - 'a';
-    mikrokopter::NcDebug debug_data;
-    debug_data.header.frame_id="mikrokopter";
-    debug_data.header.stamp=stamp;
-    debug_data.slaveAddr=slaveAddr - 'a';
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_nc_debug_message(buffer, debug_data);
-    nc_debug_data_pub.publish(debug_data);
-
-    ROS_DEBUG_STREAM( "Published NC debug package");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for nc debug msg");
-  }
-}
-void MikrokopterInterface::process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    uint8_t slaveAddr =  buffer[1] - 'a';
-    mikrokopter::FcDebug debug_data;
-    debug_data.header.frame_id="mikrokopter";
-    debug_data.header.stamp=stamp;
-    debug_data.slaveAddr=slaveAddr - 'a';
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_fc_debug_message(buffer, debug_data);
-
-    fc_debug_data_pub.publish(debug_data);
-
-    ROS_DEBUG_STREAM( "Published FC debug package");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for fc debug msg");
-  }
-}
-
-void MikrokopterInterface::process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( buffer[1] == NcAddress && isValidChecksum(buffer) ) {
-
-    mikrokopter::Navi naviMsg;
-    naviMsg.header.frame_id="mikrokopter";
-    naviMsg.header.stamp = stamp;
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_navi_message(buffer, naviMsg);
-    navi_data_pub.publish(naviMsg);
-
-    ROS_DEBUG_STREAM( "Published navi data package");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for navi data msg");
-  }
-}
-
-void MikrokopterInterface::process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( buffer[1] == FcAddress && isValidChecksum(buffer) ) {
-
-    mikrokopter::PPM ppm_data;
-    ppm_data.header.frame_id="mikrokopter";
-    ppm_data.header.stamp = stamp;
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_ppm_message(buffer, ppm_data);
-    ppm_data_pub.publish(ppm_data);
-
-    ROS_DEBUG_STREAM( "Published ppm package");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for ppm msg");
-  }
-}
-void MikrokopterInterface::process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    mikrokopter::AnalogLabel label_data;
-    label_data.header.frame_id="mikrokopter";
-    label_data.header.stamp=stamp;
-    label_data.slaveAddr   = buffer[1];
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_label_message(buffer, label_data);
-    label_data_pub.publish(label_data);
-
-    ROS_DEBUG_STREAM( "Published Analog Label message");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for analog label msg");
-  }
-}
-void MikrokopterInterface::process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    mikrokopter::Control msg;
-    msg.header.frame_id="mikrokopter";
-    msg.header.stamp=stamp;
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_control_message(buffer, msg);
-    control_data_pub.publish(msg);
-
-    ROS_DEBUG_STREAM( "Published ExternControl message");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for ExternControl msg");
-  }
-}
-void MikrokopterInterface::process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    mikrokopter::Version version_data;
-    version_data.header.frame_id="mikrokopter";
-    version_data.header.stamp=stamp;
-    version_data.slaveAddr   = buffer[1] - 'a';
-
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    extract_version_message(buffer, version_data);
-    version_data_pub.publish(version_data);
-
-    ROS_DEBUG_STREAM( "Published Version Info message");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for version info msg");
-  }
-}
-void MikrokopterInterface::process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( buffer[1] == Mk3MagAddress && isValidChecksum(buffer) )
-  {
-     mikrokopter::AngleOut msg;
-     msg.header.frame_id = "mikrokopter";
-     msg.header.stamp = stamp;
-     msg.slaveAddr = buffer[1] - 'a';
-
-     buffer.erase(buffer.begin(),buffer.begin()+3);
-     extract_angleout_message(buffer, msg);
-     angleout_data_pub.publish(msg);
-
-     ROS_DEBUG_STREAM("Published Angle Out");
-
-   } else {
-     ROS_WARN_STREAM("Checksum failed for heading msg");
-   }
-}
-
-void MikrokopterInterface::process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp)
-{
-  if ( isValidChecksum(buffer) )
-  {
-    mikrokopter::Control msg;
-    msg.header.frame_id="mikrokopter";
-    msg.header.stamp=stamp;
-
-    unsigned char outbuffer[512];
-    buffer.erase(buffer.begin(),buffer.begin()+3);
-    decode64(buffer,0,outbuffer);
-    msg.frame     = outbuffer[0];
-
-    set_control_ack_pub.publish(msg);
-
-    ROS_DEBUG_STREAM( "Published ExternControl ACK");
-
-  } else {
-    ROS_WARN_STREAM( " Checksum failed for ExternControl ACK");
-  }
-}
-
-mikrokopter::GpsPos MikrokopterInterface::processGpsPos(unsigned char* buffer)
-{
-  mikrokopter::GpsPos gps;
-  gps.longitude = (int32_t)(buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]);
-  gps.latitude = (int32_t)(buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]);
-  gps.altitude = (int32_t)(buffer[11] << 24 | buffer[10] << 16 | buffer[9] << 8 | buffer[8]);
-  gps.status = buffer[12];
-  
-  return gps;
-}
-
-mikrokopter::GpsPosDev MikrokopterInterface::processGpsPosDev(unsigned char* buffer)
-{
-  mikrokopter::GpsPosDev dev;
-  dev.distance = (uint32_t)(buffer[1] << 8 | buffer[0]);
-  dev.bearing = (int32_t)(buffer[3] << 8 | buffer[2]);
-  
-  return dev;
-}
-
-void MikrokopterInterface::readSerialCallback(const ros::TimerEvent&)
-{
-  char c;
-  static ros::Time nextStamp, stamp;
-
-  while (fd.rdbuf()->in_avail()>0 && ros::ok()) // While there is data in the serial receive buffer
-  {
-    fd.get(c);
-    buffer.push_back(c);
-
-    if (c == '#')
-    {
-      stamp = nextStamp;
-      nextStamp = ros::Time::now();
-      buffer.erase(buffer.begin(),buffer.end()-1);
-    }
-
-    if ( buffer[0] == '#' && buffer.size() >= 2)
-    {
-      if( buffer.size() == msg_len_table[buffer[2]])
-      {
-        switch (buffer[2])
-        {
-          case 'A':
-            process_label_message(buffer,stamp);
-            break;
-
-          case 'B':
-            process_external_control_ack_message(buffer,stamp);
-            break;
-
-          case 'H':
-            ROS_WARN_STREAM("Unimplemented message \"H\"");
-            break;
-
-          case 'L':
-            ROS_WARN_STREAM("Unimplemented message \"L\"");
-            break;
-
-          case 'V':
-            process_version_message(buffer,stamp);
-            break;
-
-          case 'D':
-            {
-              uint8_t slaveAddr =  buffer[1];
-              if(slaveAddr == FcAddress)
-              {
-                process_fc_debug_message(buffer,stamp);
-              }
-              else if(slaveAddr == NcAddress)
-              {
-                process_nc_debug_message(buffer,stamp);
-              }
-            }
-            break;
-
-          case 'G':
-            process_control_message(buffer,stamp);
-            break;
-
-          case 'k':
-            process_angleout_message(buffer,stamp);
-            break;
-
-          case 'P':
-            process_ppm_message(buffer,stamp);
-            break;
-
-          case 'O':
-            process_navi_message(buffer,stamp);
-            break;
-
-          default:
-            ROS_WARN_STREAM("Unknown message type: "<<buffer[2]);
-            break;
-
-        } // end switch (buffer[2])
-      } // end if( buffer.size() == msg_len_map[buffer[2]])
-    } // end if ( buffer[0] == '#')
-  } // end while (fd.rdbuf()->in_avail()>0 && ros::ok())
-}
-
-
-void MikrokopterInterface::decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr) {
-  int ptrIn=offset; 
-  int a,b,c,d,x,y,z;
-  int ptr=0;
-  int len = in_arr.size();
-  while(len!=0)
-    {
-      a = in_arr[ptrIn++] - '=';
-      b = in_arr[ptrIn++] - '=';
-      c = in_arr[ptrIn++] - '=';
-      d = in_arr[ptrIn++] - '=';
- 
-      x = (a << 2) | (b >> 4);
-      y = ((b & 0x0f) << 4) | (c >> 2);
-      z = ((c & 0x03) << 6) | d;
-
-      if((len--)!=0) out_arr[ptr++] = x; else break;
-      if((len--)!=0) out_arr[ptr++] = y; else break;
-      if((len--)!=0) out_arr[ptr++] = z; else break;
-    }
-}
-
-int MikrokopterInterface::calculateChecksum(std::vector<unsigned char>& buffer)
-{
-  int chk = 0;
-  unsigned int l=0;
-  for (l=0; l<buffer.size()-3; l++){
-    chk += buffer[l];
-  }
-  chk %= 4096;
-  return chk;
-}
-
-int MikrokopterInterface::calculateChecksum(unsigned char* buffer, int len)
-{
-  int chk = 0;
-  int l=0;
-  for (l=0; l<len-3; l++){
-    chk += buffer[l];
-  }
-  chk %= 4096;
-  return chk;
-}
-  
-bool MikrokopterInterface::isValidChecksum(std::vector<unsigned char>& buffer)
-{
-  int chk = calculateChecksum(buffer);
-  
-  // Verify the Checksum
-  int l=buffer.size()-3;
-  return (buffer[l++] == ('=' + chk / 64) && 
-	  buffer[l++] == ('=' + chk % 64) && 
-	  buffer[l++] == '\r');
-}
-
-void MikrokopterInterface::updateWithChecksum(unsigned char* buffer, int len)
-{
-  int chks = calculateChecksum(buffer, len);
-
-  buffer[len-3] = ('=' + chks / 64);
-  buffer[len-2] = ('=' + chks % 64);
-}
-
-void MikrokopterInterface::createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len)
-{
-  msglen = 0;
-  commandbuffer[msglen++] = '#';
-  commandbuffer[msglen++] = address;
-  commandbuffer[msglen++] = cmd;
-  unsigned char a,b,c;
-  a=0;b=0;c=0;
-  int ptr=0;
-  while(len)
-    {
-      if(len>0)
-	{
-	  a = data[ptr++];
-	  len--;
-	}
-      else a = 0;
-      if(len>0)
-	{
-	  b = data[ptr++];
-	  len--;
-	}
-      else b = 0;
-      if(len>0)
-	{
-	  c = data[ptr++];
-	  len--;
-	}
-      else c = 0;
-      commandbuffer[msglen++] = '=' + (a >> 2);
-      commandbuffer[msglen++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
-      commandbuffer[msglen++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
-      commandbuffer[msglen++] = '=' + ( c & 0x3f);
-    }
-  commandbuffer[msglen++] ='x';
-  commandbuffer[msglen++] ='x';
-  commandbuffer[msglen++] ='\r';
-  updateWithChecksum(commandbuffer,msglen);
-}
diff --git a/mikrokopter/src/joystick_control.cpp b/mikrokopter/src/joystick_control.cpp
deleted file mode 100644
index 2401e0e..0000000
--- a/mikrokopter/src/joystick_control.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/**
- * @brief A node responsible for reading joystick messages from ros
- *        joystick_drivers to control the MikroKopter
- *
- * @author CMU Quadrotor Project
- */
-
-#include <ros/ros.h>
-#include <sensor_msgs/Joy.h>
-#include <stdio.h>
-
-#include "nav_lib.h"
-
-class JoystickControl
-{
-    public:
-        JoystickControl();
-        void joyLoop();
-        void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
-
-    private:
-        MikrokopterControl control;
-        ros::Publisher pub;
-        ros::NodeHandle n;
-        ros::Subscriber sub;
-};
-
-JoystickControl::JoystickControl()
-{
-  sub = n.subscribe<sensor_msgs::Joy>("joy", 100,
-      &JoystickControl::joyCallback, this);
-}
-
-void JoystickControl::joyLoop()
-{
-    control.main_loop();
-}
-
-void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
-{
-    control.velocity_control(joy->axes[1], -joy->axes[0]);
-    control.set_yaw(-joy->axes[2]);
-    control.set_thrust((joy->axes[3]+1)/2); ///Adjust thrust to be between [0, 1] instead of [-1, 1]
-    return;
-}
-
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "joystick_control");
-    JoystickControl joy_control;
-
-    // This commands loops infinitely
-    joy_control.joyLoop();
-
-    return 0;
-}
diff --git a/mikrokopter/src/keyboard_control.cpp b/mikrokopter/src/keyboard_control.cpp
deleted file mode 100644
index a0d19ce..0000000
--- a/mikrokopter/src/keyboard_control.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/**
- * @brief A node responsible for keyboard commands which will be sent to
- *        the MikroKopter
- *
- * @author CMU Quadrotor Project
- * @author Alex Zirbel
- */
-
-#include <ros/ros.h>
-#include <signal.h>
-#include <termios.h>
-#include <stdio.h>
-#include <ncurses.h>
-
-#include "nav_lib.h"
-
-#define KEYCODE_R 0x43 
-#define KEYCODE_L 0x44
-#define KEYCODE_U 0x41
-#define KEYCODE_D 0x42
-#define KEYCODE_Q 0x71
-
-class KeyboardControl
-{
-    public:
-        KeyboardControl();
-        void keyLoop();
-
-    private:
-        ros::NodeHandle n;
-};
-
-KeyboardControl::KeyboardControl()
-{
-}
-
-void KeyboardControl::keyLoop()
-{
-    
-    ros::NodeHandle n;
-    ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
-
-    MikrokopterControl control;
-
-    ros::Rate loop_rate(25);
-    
-    char c;
-
-    // get the console in raw mode
-    initscr();
-    timeout(0);
-    noecho();
-    cbreak();
-
-    puts("Reading from keyboard");
-    puts("---------------------------");
-    puts("Use arrow keys to command the quadrotor.");
-
-    while(ros::ok())
-    {
-        // get the next event from the keyboard  
-        c = getch();
-
-        //ROS_INFO("key press: 0x%02X\n", c);
-        //ROS_INFO("key press: %c\n", c);
-
-        switch(c)
-        {
-            case KEYCODE_L:
-            case 'a':
-                ROS_INFO("A");
-                control.left();
-                break;
-            case KEYCODE_R:
-            case 'd':
-                ROS_INFO("D");
-                control.right();
-                break;
-            case KEYCODE_U:
-            case 'w':
-                ROS_INFO("W");
-                control.forward();
-                break;
-            case KEYCODE_D:
-            case 's':
-                ROS_INFO("B");
-                control.backward();
-                break;
-            case ' ':
-                ROS_INFO("space");
-                control.level();
-                break;
-            case 'r':
-                ROS_INFO("UP");
-                //set_throttle(&pub, );
-                break;
-            case 'f':
-                ROS_INFO("DOWN");
-                //set_throttle(&pub);
-                break;
-            default:
-                control.level();
-        }
-
-        control.publish_on(pub);
-        ros::spinOnce();
-        loop_rate.sleep();
-    }
-    
-    endwin();
-    return;
-}
-
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "keyboard_control");
-    KeyboardControl teleop_turtle;
-
-    // This commands loops infinitely
-    teleop_turtle.keyLoop();
-
-    return(0);
-}
-
-
diff --git a/mikrokopter/src/mikrokopter_node.cpp b/mikrokopter/src/mikrokopter_node.cpp
deleted file mode 100644
index ef6ae9c..0000000
--- a/mikrokopter/src/mikrokopter_node.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
- /*!
-  *  \file    mikrokopter_node.cpp
-  *  \brief   Instantiates the MikrokopterInterface class
-  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
-  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
-  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
-  *  \date    2011 October 25
-  *  \details This program is a ROS wrapper for the MikrokopterInterface
-  */
-
-#include <ros/ros.h>
-#include <mikrokopter/interface.h>
-
-using namespace CA;
-
-int main(int argc, char **argv)
-{
-   ros::init(argc, argv, "mikrokopter");
-   ros::NodeHandle n;
- 
-   MikrokopterInterface interface(n);
-   ros::spin();
-}
diff --git a/mikrokopter/src/mk_wrapper.cpp b/mikrokopter/src/mk_wrapper.cpp
deleted file mode 100644
index 0bb7f3c..0000000
--- a/mikrokopter/src/mk_wrapper.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-#include <ros/ros.h>
-#include <mikrokopter/Control.h>
-
-class MKWrapper
-{
-public:
-    MKWrapper();
-    void main_loop();
-    void control_callback(const mikrokopter::Control& control);
-    
-private:
-    ros::Publisher pub;
-    ros::Subscriber sub;
-    ros::NodeHandle nh;
-    ros::Duration max_quiet_time;
-    ros::Time last_time_recv;
-    bool started, dead;
-};
-
-MKWrapper::MKWrapper()
-{
-    pub = nh.advertise<mikrokopter::Control>("/mikrokopter/req_set_control",
-        100); 
-    sub = nh.subscribe("/mk_wrapper/control", 100, &MKWrapper::control_callback,
-        this);
-    int max_quiet_time_ms;
-    ros::param::param<int>("~max_quiet_time", max_quiet_time_ms, 1000);
-    ROS_INFO("Max quiet time: %d", max_quiet_time_ms);
-    max_quiet_time.fromSec(max_quiet_time_ms / 1000.);
-    started = false;
-    dead = false;
-}
-
-void MKWrapper::main_loop()
-{
-    mikrokopter::Control control_dead;
-    control_dead.pitch = 0;
-    control_dead.roll = 0;
-    control_dead.yaw = 0;
-    control_dead.thrust = 0;
-    
-    ros::Rate loop_rate(20);
-    while (ros::ok())
-    {
-        if (started)
-        {
-            if (dead)
-            {
-                pub.publish(control_dead);
-            }
-            else if (ros::Time::now() - last_time_recv > max_quiet_time)
-            {
-                dead = true;
-                ROS_ERROR("Lost connection: mk_wrapper must be restarted");
-            }
-        }
-        loop_rate.sleep();
-        ros::spinOnce();
-    }
-}
-
-void MKWrapper::control_callback(const mikrokopter::Control& control)
-{
-    started = true;
-    if (!dead)
-    {
-        pub.publish(control);
-        last_time_recv = ros::Time::now();
-    }
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "mk_wrapper");
-    MKWrapper wrapper;
-    wrapper.main_loop();
-    return 0;
-}
diff --git a/mikrokopter/src/nav_lib.cpp b/mikrokopter/src/nav_lib.cpp
deleted file mode 100644
index 9f1030f..0000000
--- a/mikrokopter/src/nav_lib.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "nav_lib.h"
-
-MikrokopterControl::MikrokopterControl()
-{
-    control.digital[0] = 0;
-    control.digital[1] = 0;
-    control.remoteKey = 0;
-    control.pitch = 0;
-    control.roll = 0;
-    control.yaw = 0;
-    control.thrust = 40;
-    control.height = 0;
-    control.free = 0;
-    control.frame = 7;
-    control.config = 1;
-}
-
-void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed)
-{
-    if(forward_speed > 1) forward_speed = 1;
-    if(forward_speed < -1) forward_speed = -1;
-    if(lateral_speed > 1) lateral_speed = 1;
-    if(lateral_speed < -1) lateral_speed = -1; 
-    int k = 50; // TODO set scaling constant and check signs
-    control.roll = k*(forward_speed - lateral_speed);
-    control.pitch = k*(forward_speed + lateral_speed);
-}
-
-void MikrokopterControl::forward()
-{
-    control.roll = 15;
-    control.pitch = 15;
-}
-
-void MikrokopterControl::backward()
-{
-    control.roll = -15;
-    control.pitch = -15;
-}
-
-void MikrokopterControl::left()
-{
-    control.roll = 15;
-    control.pitch = -15;
-}
-
-void MikrokopterControl::right()
-{
-    control.roll = -15;
-    control.pitch = 15;
-}
-
-void MikrokopterControl::level()
-{
-    control.roll = 0;
-    control.pitch = 0;
-}
-
-void MikrokopterControl::set_thrust(float thrust)
-{
-    if (thrust < 0) thrust = 0;
-    if (thrust > 1) thrust = 1;
-    int k = 255;
-    control.thrust = k*thrust;
-}
-
-void MikrokopterControl::set_yaw(float yaw)
-{
-    if(yaw>1) yaw = 1;
-    if(yaw<-1) yaw = -1;
-    int k = 50;;
-    control.yaw = k*yaw;
-}
-
-void MikrokopterControl::publish_on(ros::Publisher& pub)
-{
-    pub.publish(control);
-}
-
-void MikrokopterControl::main_loop()
-{
-    ros::Publisher pub = nh.advertise< ::mikrokopter::Control>(
-        "/mk_wrapper/control", 200);
-    
-    int publish_rate;
-    ros::param::param<int>("~publish_rate", publish_rate, 20);
-    ros::Rate loop_rate(publish_rate);
-    
-    while (ros::ok())
-    {
-        ros::spinOnce();
-        pub.publish(control);
-        loop_rate.sleep();
-    }
-    
-}
diff --git a/mikrokopter/src/nav_lib.h b/mikrokopter/src/nav_lib.h
deleted file mode 100644
index 5e02bee..0000000
--- a/mikrokopter/src/nav_lib.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#include "ros/ros.h"
-#include "mikrokopter/Control.h"
-
-class MikrokopterControl {
-public:
-    MikrokopterControl();
-    void velocity_control(float forward_speed, float lateral_speed);
-    void forward();
-    void backward();
-    void left();
-    void right();
-    void level();
-    void set_thrust(float thrust);
-    void set_yaw(float yaw);
-    void publish_on(ros::Publisher& pub);
-    void main_loop();
-private:
-    mikrokopter::Control control;
-    ros::NodeHandle nh;
-};
diff --git a/mikrokopter/src/node_control.cpp b/mikrokopter/src/node_control.cpp
deleted file mode 100644
index 6145c1c..0000000
--- a/mikrokopter/src/node_control.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#include "ros/ros.h"
-#include "std_msgs/Float64.h"
-#include "mikrokopter/Velocity2D.h"
-#include "nav_lib.h"
-
-class NodeControl
-{
-public:
-  NodeControl();
-  void main_loop() {control.main_loop();}
-  void cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n);
-  void cb_yaw(const std_msgs::Float64::ConstPtr& n);
-  void cb_thrust(const std_msgs::Float64::ConstPtr& n);
-
-private:
-  MikrokopterControl control;
-  ros::NodeHandle n;
-  ros::Subscriber sub_velocity;
-  ros::Subscriber sub_yaw;
-  ros::Subscriber sub_thrust;
-};
-
-NodeControl::NodeControl()
-{
-  sub_velocity = n.subscribe<mikrokopter::Velocity2D>("/mikrokopter/velocity",
-      100, &NodeControl::cb_velocity, this);
-  sub_yaw = n.subscribe<std_msgs::Float64>("/mikrokopter/yaw", 100,
-      &NodeControl::cb_yaw, this);
-  sub_thrust= n.subscribe<std_msgs::Float64>("/mikrokopter/thrust", 100,
-      &NodeControl::cb_thrust, this);
-}
-
-void NodeControl::cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n)
-{
-  control.velocity_control(n->forward, n->lateral);
-}
-void NodeControl::cb_yaw(const std_msgs::Float64::ConstPtr& n)
-{
-  control.set_yaw(n->data);
-}
-void NodeControl::cb_thrust(const std_msgs::Float64::ConstPtr& n)
-{
-  control.set_thrust(n->data);
-}
-
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "node_control");
-  NodeControl control = NodeControl();
-  control.main_loop();
-  return 0;
-}
diff --git a/mikrokopter/src/turn_to_target.cpp b/mikrokopter/src/turn_to_target.cpp
deleted file mode 100644
index 5714638..0000000
--- a/mikrokopter/src/turn_to_target.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#include "nav_lib.h"
-#include <geometry_msgs/Point.h>
-#include <math.h>
-
-class TurnToTarget
-{
-public:
-  TurnToTarget();
-  void mainLoop();
-  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
-
-private:
-  MikrokopterControl control;
-  ros::Publisher pub;
-  ros::NodeHandle n;
-  ros::Subscriber sub;
-};
-
-TurnToTarget::TurnToTarget()
-{
-  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
-  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
-      &TurnToTarget::pointCallback, this);
-}
-
-void TurnToTarget::mainLoop()
-{
-  /*ros::Rate loop_rate(25);
-  while(ros::ok())
-  {
-    ros::spinOnce();
-    control.publish_on(pub);
-    loop_rate.sleep();
-  }*/
-  control.main_loop();
-}
-
-void TurnToTarget::pointCallback(const geometry_msgs::Point::ConstPtr& p)
-{
-  // angle going CCW from forward in horizontal plane
-  float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
-  // should be from [-100, 100]
-  control.set_yaw(angle*100/M_PI);
-}
-
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "turn_to_target");
-  TurnToTarget ttt = TurnToTarget();
-  ttt.mainLoop();
-  return 0;
-}
