diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/main.cpp | 79 | ||||
| -rw-r--r-- | src/main.h | 8 |
2 files changed, 83 insertions, 4 deletions
diff --git a/src/main.cpp b/src/main.cpp index d8dfdb9..150a141 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,6 +20,59 @@ */ #include "main.h" +bool seenutm=false; +bool seenpva=false; + +#ifdef USE_ROS + +#else /* ----- not USE_ROS ----- */ +void +imgCallback(const message *msg) +{ + return; +} +void +imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q) +{ + if (!seenutm || !seenpva) return; + using Eigen::Vector3d; + Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z); + Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z); +} + +void +pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q) +{ + using Eigen::AngleAxisd; + using Eigen::Vector3d; + double roll, pitch, yaw; + attitude_t A=msg.attitude; + roll = M_PI*msg.attitude.roll/180.; + pitch = M_PI*msg.attitude.pitch/180.; + yaw = M_PI*(90.-msg.attitude.yaw)/180.; + Eigen::Matrix3d R; + R = AngleAxisd(yaw, Vector3d::UnitZ()) + * AngleAxisd(roll, Vector3d::UnitY()) + * AngleAxisd(pitch, Vector3d::UnitX()); + q = Eigen::Quaternion<double>(R); + if (!seenpva) { + Vector3d vw(msg.velocity.east,msg.velocity.north,msg.velocity.up); + X.segment(3,3) = R.transpose()*vw; + seenpva=true; + } + return; +} + +void +utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X) +{ + seenutm=true; + X[0] = msg.utm.easting; + X[1] = msg.utm.northing; + X[2] = msg.utm.up; + return; +} +#endif /* ----- not USE_ROS ----- */ int parseLine(char *line, message *data) @@ -85,6 +138,8 @@ parseLine(char *line, message *data) int main(int argc, char **argv) { + Eigen::Quaternion<double> qbw; + Eigen::Matrix<double,9,1> bodyStateVector; #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ @@ -93,8 +148,28 @@ char line[MAXLINE]; while (scanf("%s", line)!=EOF) { message msg; parseLine(line, &msg); - if (msg.msg_type==INSPVAS) { - } + + switch ( msg.msg_type ) { + case BESTUTM: + utmCallback(msg,bodyStateVector); + break; + + case IMG: + imgCallback(&msg); + break; + + case INSPVAS: + pvaCallback(msg, bodyStateVector, qbw); + std::cout << bodyStateVector << std::endl; + break; + + case RAWIMUS: + imuCallback(msg, bodyStateVector, qbw); + break; + + default: + break; + } /* ----- end switch ----- */ } #endif /* ----- not USE_ROS ----- */ return 0; @@ -8,8 +8,9 @@ #include <cstring> #endif /* ----- USE_ROS ----- */ -#include <iostream> +#include <cmath> #include <Eigen/Dense> +#include <iostream> #define MAXLINE 8192 #define MAXFILENAME 1024 @@ -73,7 +74,10 @@ int parseLine(char *line, message *msg); #ifdef USE_ROS void imuCallback(); #else /* ----- not USE_ROS ----- */ -void imuCallback(const message *msg); +void imgCallback(const message *msg); +void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q); +void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q); +void utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X); #endif /* ----- not USE_ROS ----- */ #endif /* ----- #ifndef main_INC ----- */ |