diff --git a/src/axis_imu.cc b/src/axis_imu.cc
index 5cfc779502c036ed1df85002611dcfb7846ce426..351194e0cec376ae2063b3403df651fba03d1660 100644
--- a/src/axis_imu.cc
+++ b/src/axis_imu.cc
@@ -46,7 +46,7 @@ void spin_thread () {
 }
 
 float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2,  unsigned char b3) { 
-  //  unsigned char byte_array[] = { b3, b2, b1, b0 };
+  // unsigned char byte_array[] = { b3, b2, b1, b0 };
   unsigned char byte_array[] = { b0, b1, b2, b3 };
   float result;
   std::copy(reinterpret_cast<const char*>(&byte_array[0]),
@@ -63,6 +63,7 @@ public:
     std::cout << "UDP server listening on " << port_number << std::endl;
     have_gyro_flag = false;
     have_acc_flag = false;
+    have_pos_flag = false;
     start_receive();
   }
 
@@ -96,15 +97,16 @@ private:
   void handle_receive(const boost::system::error_code& error,
                       std::size_t bytes_transferred) {
     if (!error || error == boost::asio::error::message_size) {
+      // cerr << "Got message: " << bytes_transferred << endl;
       //      cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << " - " << recv_buffer_[33] << endl;
       //      float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
       double gtime = bytesToDouble(1);
       //float ax, ay, az, gx, gy, gz;
       double ax, ay, az, gx, gy, gz;
       // Should be in rad/s
-      gx = bytesToDouble(9)/180.0*M_PI;
-      gy = bytesToDouble(17)/180.0*M_PI;
-      gz = bytesToDouble(25)/180.0*M_PI;
+      gx = bytesToDouble(9);
+      gy = bytesToDouble(17);
+      gz = bytesToDouble(25);
 
       double atime = bytesToDouble(34);
       // Accel should be in m/s²
@@ -120,6 +122,18 @@ private:
         // ROS_ERROR("ACCELEROMETER: %f - %f %f %f", atime, ax, ay, az);
       }
 
+      double ptime = 0.0;
+      double pan = 0.0;
+      double tilt = 0.0;
+
+      if (recv_buffer_[66] == 'P') {
+        have_pos_flag = true;
+        ptime = bytesToDouble(67);
+        pan = bytesToDouble(75);
+        tilt = bytesToDouble(83);
+        ROS_ERROR("POSITION TIME - PAN TILT: %f - %f %f", pan, tilt);
+      }
+
       if (!have_gyro_flag) {
         start_receive();        
         return;
@@ -206,6 +220,7 @@ private:
 
   bool have_gyro_flag;
   bool have_acc_flag;
+  bool have_pos_flag;
 };
   
 void runUDPServer(int port) {