diff --git a/src/axis_imu.cc b/src/axis_imu.cc
index d64893de49b327cbc0c6d2e7f9677500d550bf81..9027234b6ad68a2e4f6a755d810462ab9d408f37 100644
--- a/src/axis_imu.cc
+++ b/src/axis_imu.cc
@@ -53,16 +53,6 @@ float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2,  unsign
   return result;
 } 
 
-double bytesToDouble(unsigned char b0, unsigned char b1, unsigned char b2,  unsigned char b3,
-                     unsigned char b4, unsigned char b5, unsigned char b6,  unsigned char b7) { 
-  //unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
-  unsigned char byte_array[] = { b7, b6, b5, b4, b3, b2, b1, b0 }; //netw byte order
-  double result;
-  std::copy(reinterpret_cast<const char*>(&byte_array[0]),
-            reinterpret_cast<const char*>(&byte_array[8]),
-            reinterpret_cast<char*>(&result));
-  return result;
-} 
 
 class udp_server {
 public:
@@ -75,6 +65,25 @@ public:
   }
 
 private:
+
+  double bytesToDouble(unsigned int i) { 
+  //unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
+    unsigned char byte_array[] = { recv_buffer_[i+7],
+                                   recv_buffer_[i+6],
+                                   recv_buffer_[i+5],
+                                   recv_buffer_[i+4],
+                                   recv_buffer_[i+3],
+                                   recv_buffer_[i+2],
+                                   recv_buffer_[i+1],
+                                   recv_buffer_[i+0]}; //netw byte order
+    double result;
+    std::copy(reinterpret_cast<const char*>(&byte_array[0]),
+              reinterpret_cast<const char*>(&byte_array[8]),
+              reinterpret_cast<char*>(&result));
+    return result;
+  } 
+
+  
   void start_receive() {
     socket_.async_receive_from(boost::asio::buffer(recv_buffer_), remote_endpoint_,
                                boost::bind(&udp_server::handle_receive, this,
@@ -87,30 +96,25 @@ private:
     if (!error || error == boost::asio::error::message_size) {
       // cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << endl;
       //      float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
-      double time = bytesToDouble(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4],
-                                  recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
+      double gtime = bytesToDouble(1);
       //float ax, ay, az, gx, gy, gz;
-      double ax, ay, az, gx, gy, gz, v0, v1, v2;
-      v0 = bytesToDouble(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12],
-                         recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
-      v1 = bytesToDouble(recv_buffer_[17], recv_buffer_[18], recv_buffer_[19], recv_buffer_[20],
-                         recv_buffer_[21], recv_buffer_[22], recv_buffer_[23], recv_buffer_[24]);
-      v2 = bytesToDouble(recv_buffer_[25], recv_buffer_[26], recv_buffer_[27], recv_buffer_[28],
-                         recv_buffer_[29], recv_buffer_[30], recv_buffer_[31], recv_buffer_[32]);
-      if (recv_buffer_[0] == 'A') {
-        have_acc_flag = true;
-        ax = v0;
-        ay = v1;
-        az = v2;
-        ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
-      }
+      double ax, ay, az, gx, gy, gz;
+      gx = bytesToDouble(9);
+      gy = bytesToDouble(17);
+      gz = bytesToDouble(25);
+
+      double atime = bytesToDouble(33);
+      ax = bytesToDouble(41);
+      ay = bytesToDouble(49);
+      az = bytesToDouble(57);
       if (recv_buffer_[0] == 'G') {
         have_gyro_flag = true;
-        gx = v0;
-        gy = v1;
-        gz = v2;
         ROS_ERROR("         GYRO: %f - %f %f %f", time, gx, gy, gz);        
       }
+      if (recv_buffer_[32] == 'A') {
+        have_acc_flag = true;
+        ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
+      }
 
       if (!have_gyro_flag) {
         start_receive();