diff --git a/CMakeLists.txt b/CMakeLists.txt
index 212386fe9c39a5fa8804e8ba83d7b6af144d7f80..4a4c54afc52f41b7b012770537de97e7f8e7790b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,8 +17,8 @@ IF (NOT CMAKE_BUILD_TYPE)
  SET(CMAKE_BUILD_TYPE "DEBUG") 
 ENDIF (NOT CMAKE_BUILD_TYPE)
 
-SET(CMAKE_C_FLAGS_DEBUG "-g -Wall")
-SET(CMAKE_C_FLAGS_RELEASE "-O3")
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall")
+SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
 ADD_SUBDIRECTORY(src)
 
diff --git a/src/examples/motionModelData.cpp b/src/examples/motionModelData.cpp
index 4df786ea5d9c578eb4c6536d8c96e7708e95b247..bfae9a3f88083f84039ded7b97a8872c6bddc606 100644
--- a/src/examples/motionModelData.cpp
+++ b/src/examples/motionModelData.cpp
@@ -56,7 +56,6 @@ int main(int argc, char *argv[])
 {
 	//operational variables
 	CSegwayRMP200 *segway;
-	CFTDIServer *ftdi_server=CFTDIServer::instance();
 	string serial_number;
 	unsigned int ii=0;
 	unsigned int jj=0;
diff --git a/src/examples/test_segwayRMP200.cpp b/src/examples/test_segwayRMP200.cpp
index 7acc1bb37697133a387351c54daebd772de5f85a..482b517c7e36a1235c947afdbd6df9affac5917b 100755
--- a/src/examples/test_segwayRMP200.cpp
+++ b/src/examples/test_segwayRMP200.cpp
@@ -49,7 +49,6 @@ int main(int argc, char *argv[])
 {
   CSegwayRMP200 *segway = NULL;
   std::list<std::string> events;
-  CFTDIServer *ftdi_server=CFTDIServer::instance();
   CEventServer *event_server=CEventServer::instance();
   std::string serial_number,cable_disconnected,power_off;
   bool connected=false;
diff --git a/src/segway_rmp200.cpp b/src/segway_rmp200.cpp
index f6a18e61321feb6844b2767ce67d28c64b42b6c3..d9454b0dc8de35a1ce0c1da79f4c3bc6c7e250f5 100755
--- a/src/segway_rmp200.cpp
+++ b/src/segway_rmp200.cpp
@@ -256,7 +256,6 @@ bool CSegwayRMP200::read_packet(segway_packet *packet,int *packet_len)
 void CSegwayRMP200::parse_packet(segway_packet *packet)
 {
   short int command;
-  int i=0;
 
   if(this->compute_checksum(packet)!=packet->data[17])
   {
@@ -314,6 +313,8 @@ void *CSegwayRMP200::start_read_thread(void *param)
   CSegwayRMP200 *segway = (CSegwayRMP200 *) param;
 
   segway->read_thread();
+
+  pthread_exit(NULL);
 }
 
 void CSegwayRMP200::read_thread(void)
@@ -343,8 +344,6 @@ void CSegwayRMP200::read_thread(void)
       end=true; 
     }
   }
-
-  pthread_exit(NULL);
 }
 
 void *CSegwayRMP200::start_command_thread(void *param)
@@ -352,11 +351,13 @@ void *CSegwayRMP200::start_command_thread(void *param)
   CSegwayRMP200 *segway = (CSegwayRMP200 *) param;
 
   segway->command_thread();
+
+  pthread_exit(NULL);
 }
  
 void CSegwayRMP200::command_thread(void)
 {
-  segway_packet packet={0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x13,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+  segway_packet packet={{0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x13,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}};
   std::vector<unsigned char> command;
   int exception_count=0;
   bool end=false;
@@ -413,8 +414,6 @@ void CSegwayRMP200::command_thread(void)
       end=true;
     }
   }
-
-  pthread_exit(NULL);
 }
 
 void *CSegwayRMP200::heartbeat_thread(void *param)
@@ -673,9 +672,6 @@ TSegwayRMP200Status CSegwayRMP200::get_status(void)
     status.right_torque         = this->right_torque;
     status.ui_battery           = this->ui_battery;
     status.powerbase_battery    = this->powerbase_battery;
-    op_mode operation_mode      = this->mode;
-    op_mode hardware_mode       = this->hardware_mode;
-    gain gain_schedule          = this->gain_schedule;
 
     return status;
 }
@@ -844,7 +840,7 @@ void CSegwayRMP200::connect(const std::string& desc_serial)
 
 void CSegwayRMP200::reset(void)
 {
-  segway_packet packet={0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+  segway_packet packet={{0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}};
 
   if(this->comm_dev==NULL)
   {