diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/main.cpp | 18 | ||||
| -rw-r--r-- | src/main.h | 2 |
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; @@ -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> |