Work on axis imu.

README.md

@@ -14,6 +14,6 @@ rosrun axis_imu axis_imu _frame_id:=imu_frame _rate:=100 _root_passwd:=pass _cam
 
 The orientation is not filled in since I do not get sensible values
 from my camera. The POSITION is really the pan tilt value so should
-not be used as imu rotaation.
+not be used as imu rotation.
 
 Patch the code if you need.

src/axis_imu.cc

@@ -5,6 +5,7 @@
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/MagneticField.h"
 
+#include <fstream>
 #include <iostream>
 #include <exception>
 #include <boost/array.hpp>
@@ -22,6 +23,12 @@ ros::Publisher imu_pub;
 sensor_msgs::Imu imu_msg;
 boost::mutex mutex;
 
+bool verbose=false;
+int xgyro = 0;
+int xacc = 0;
+double first_time = -1.0;
+//std::ofstream gplotfile;
+//std::ofstream aplotfile;
 
 // Data sent in network byte order
 //
@@ -60,7 +67,9 @@ class udp_server {
 public:
   udp_server(boost::asio::io_service& io_service, int port_number)
     : socket_(io_service, boost::asio::ip::udp::udp::endpoint(boost::asio::ip::udp::udp::v4(), port_number)) {
-    std::cout << "UDP server listening on " << port_number << std::endl;
+    if (verbose) {
+      std::cout << "UDP server listening on " << port_number << std::endl;
+    }
     have_gyro_flag = false;
     have_acc_flag = false;
     have_pos_flag = false;
@@ -117,11 +126,20 @@ private:
       az = bytesToDouble(58);
       if (recv_buffer_[0] == 'G') {
         have_gyro_flag = true;
-        ROS_INFO("         GYRO: %f - %f %f %f", gtime, gx, gy, gz);        
+        if (verbose) {
+          ROS_INFO("         GYRO: %f - %f %f %f", gtime, gx, gy, gz);
+        }
+        if (first_time < 0) {
+          first_time = gtime;
+        }
+        // gplotfile << xgyro++ << ", " << std::setprecision (17) << (gtime-first_time) << std::endl;
       }
       if (recv_buffer_[33] == 'A') {
         have_acc_flag = true;
-        ROS_INFO("ACCELEROMETER: %f - %f %f %f", atime, ax, ay, az);
+        if (verbose) {
+          ROS_INFO("ACCELEROMETER: %f - %f %f %f", atime, ax, ay, az);
+        }
+        // aplotfile << xacc++ << ", " << std::setprecision (17) << (atime-first_time) << std::endl;
       }
 
       double ptime = 0.0;
@@ -133,7 +151,9 @@ private:
         ptime = bytesToDouble(67);
         pan = bytesToDouble(75);
         tilt = bytesToDouble(83);
-        ROS_INFO("POSITION TIME - PAN TILT: %f - %f %f", pan, tilt);
+        if (verbose) {
+          ROS_INFO("POSITION TIME - PAN TILT: %f - %f %f", pan, tilt);
+        }
       }
 
       if (!have_gyro_flag) {
@@ -252,6 +272,9 @@ int main(int argc, char *argv[]) {
   ros::init(argc, argv, "axis_imu");
   ros::NodeHandle nh, private_nh("~");
 
+  //  gplotfile.open("plotgyro.xy");
+  //  aplotfile.open("plotacc.xy");
+
   int port;
   int rate;
   int pos_enabled;
@@ -261,12 +284,30 @@ int main(int argc, char *argv[]) {
 
   private_nh.param<int>("port", port, 5000);
   private_nh.param<std::string>("frame_id", frame_id, "imu");
-  private_nh.param<int>("rate", rate, 200);
+  private_nh.param<int>("rate", rate, 1000);
   private_nh.param<int>("pos_enabled", pos_enabled, 0);  
   private_nh.param<std::string>("camera_ip", camera_ip, "192.168.7.19");
   private_nh.param<std::string>("driver_ip", driver_ip, "192.168.7.160");
   private_nh.param<std::string>("root_passwd", root_passwd, "pass");
 
+  int gyro_div = 3;
+  int accel_div = 2;
+
+  if (rate == 1000) {
+    gyro_div = 1;
+    accel_div = 0;
+  }
+
+  if (rate == 500) {
+    gyro_div = 2;
+    accel_div = 1;
+  }
+
+  if (rate == 250) {
+    gyro_div = 3;
+    accel_div = 2;
+  }
+
   ROS_INFO("  Camera IP: %s", camera_ip.c_str());
   ROS_INFO("  Driver IP: %s", driver_ip.c_str());
   ROS_INFO("       Port: %d", port);
@@ -274,14 +315,23 @@ int main(int argc, char *argv[]) {
   ROS_INFO("       Rate: %d", rate);
   ROS_INFO("Pos Enabled: %d", pos_enabled);
   ROS_INFO("  Root pass: %s", root_passwd.c_str());  
+  ROS_INFO("   gyro_div: %d", gyro_div);
+  ROS_INFO("  accel_div: %d", accel_div);
 
-  int gyro_div = 2000/rate;
-  int accel_div = 1000/rate;
+  assert (gyro_div == (accel_div + 1));
 
   // currently fixed rate out so set div to 0
 
-  gyro_div = 1;
-  accel_div = 1;
+  // gyro_div must be accel_div + 1 (gyrodiv = accel_div + 1)
+
+  //  gyro_div = 3;         // 0 = 2000 Hz, not consistent with doc
+                        // 1 = 1000 Hz
+                        // 2 = 500 Hz
+                        // 3 = 250 Hz
+  
+  //  accel_div = 2;        // 0 = 500 Hz
+                        // 1 = 1000 HZ
+                        // 2 = 250 Hz
 
   std::ostringstream os;
   std::ostringstream osstop;