diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 14 |
1 files changed, 10 insertions, 4 deletions
diff --git a/src/main.cpp b/src/main.cpp index 11bf6dd..fa0b5fd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -57,6 +57,7 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) #endif { if (seencov && seenutm && seenpva) return; + if (!seenpva) return; seencov=true; // Rotation from body to world #if STATESIZE==13 @@ -153,7 +154,7 @@ imuCallback(const message &msg, State &mu, const timestamp dt) imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt) #endif { - if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return; + if (dt.secs==0 && dt.nsecs==0) return; Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z); Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z); // WARNING: This is the bias for 1112-1 @@ -190,6 +191,7 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) * AngleAxisd(roll, Vector3d::UnitX()) * AngleAxisd(pitch, Vector3d::UnitY()); q = Quaterniond(R); + //cout <<q.x() <<" " << q.y() << " " << q.z() << " " << q.w() << endl; // Set body velocity if (!seenpva || !seencov || !seenutm) { @@ -396,7 +398,11 @@ main(int argc, char **argv) ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100); #else /* ----- not ROS_PUBLISH ----- */ // Print unicsv header - printf("GPS,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); +#if STATESIZE==13 + printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n"); +#else + 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 #endif /* ----- not ROS_PUBLISH ----- */ // Read sensors from file int i=0; @@ -412,8 +418,7 @@ main(int argc, char **argv) #else utmCallback(msg, mu,qbw); #endif - printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); - mu.unicsv(); + //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9); break; case IMG: @@ -426,6 +431,7 @@ main(int argc, char **argv) case INSPVAS: pvaCallback(msg, mu, qbw); + if (seenutm) mu.unicsv(); break; case RAWIMUS: |