Adapting to use of double.

src/axis_imu.cc

@@ -18,7 +18,7 @@ std::string frame_id;
 
 ros::Publisher imu_pub;
 
-// 1 byte header ('A' eller 'G') | float32 timestamp | float32 x | float32 y | float32 z
+// 1 byte header ('A' eller 'G') | float64 timestamp | float64 x | float64 y | float64 z
 //
 // Headern 'A' och 'G' står för accelerometer respektive gyro.
 // timestamp är sekunder sedan kamerans uppstart
@@ -50,6 +50,17 @@ 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[] = { b3, b2, b1, b0 };
+  unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
+  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:
   udp_server(boost::asio::io_service& io_service, int port_number)
@@ -72,20 +83,29 @@ private:
                       std::size_t bytes_transferred) {
     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]);
-      float ax, ay, az, gx, gy, gz;
+      //      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]);
+      //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 = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
-        ay = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
-        az = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
+        ax = v0;
+        ay = v1;
+        az = v2;
         ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
       }
       if (recv_buffer_[0] == 'G') {
         have_gyro_flag = true;
-        gx = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
-        gy = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
-        gz = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
+        gx = v0;
+        gy = v1;
+        gz = v2;
         ROS_ERROR("         GYRO: %f - %f %f %f", time, gx, gy, gz);        
       }