Update main

2 files changed, 13 insertions, 7 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 9de28ff..246f73d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,7 +31,7 @@ double
 aboveWater(const Quaterniond &q)
 {
     Vector3d tip;
-    tip << CANOECENTER*3.43, 0, -0.65;
+    tip << CANOECENTER*3.43, 0, CANOEHEIGHT;
     tip = q._transformVector(tip);
     return tip[2];
 }
@@ -189,6 +189,7 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q)
     } else {
         i+=1;
         //if ( i==20) {
+        //if (i%18==0) {
         if (false) {
             UTM utm_water;
             utm_water.northing = msg.utm.northing;
@@ -354,6 +355,7 @@ main(int argc, char **argv)
     printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
 #endif     /* -----  not ROS_PUBLISH  ----- */
     // Read sensors from file
+    int i=0;
     char line[MAXLINE];
     while (scanf("%s", line)!=EOF) {
         message msg;
@@ -373,15 +375,17 @@ main(int argc, char **argv)
                 break;
 
             case RAWIMUS:	
-                dt = update_dt(msg.stamp, &t_old);
-                imuCallback(msg, mu, qbw, dt);
-                if (seenutm && seencov && seenpva) {
+                if (i++%DOWNSAMPLE==0) {
+                    dt = update_dt(msg.stamp, &t_old);
+                    imuCallback(msg, mu, qbw, dt);
+                    if (seenutm && seencov && seenpva) {
 #ifdef  ROS_PUBLISH
-                    geometry_msgs::PoseStamped msg;
-                    ros::spinOnce();
+                        geometry_msgs::PoseStamped msg;
+                        ros::spinOnce();
 #else
-                    mu.unicsv();
+                        mu.unicsv();
 #endif
+                    }
                 }
                 break;
 
diff --git a/src/main.h b/src/main.h
index ac2bc2a..46fb2f6 100644
--- a/src/main.h
+++ b/src/main.h
@@ -35,6 +35,8 @@
 //#define ACCBIASZ -1*-0.00709229
 
 #define CANOECENTER 0.65            /* center of gravity of canoe  */
+#define CANOEHEIGHT -0.55
+#define DOWNSAMPLE 1            /*  */
 
 #if  ROS_PUBLISH
 #include <ros/ros.h>