Go to the documentation of this file.00001
00026 #ifndef _MICROSTRAIN_3DM_GX5_45_H
00027 #define _MICROSTRAIN_3DM_GX5_45_H
00028
00029
00030 extern "C" {
00031 #include "mip_sdk.h"
00032 #include "byteswap_utilities.h"
00033 #include "mip_gx4_imu.h"
00034 #include "mip_gx4_45.h"
00035 }
00036
00037 #include <cstdio>
00038 #include <unistd.h>
00039
00040
00041
00042 #include "ros/ros.h"
00043 #include "sensor_msgs/NavSatFix.h"
00044 #include "sensor_msgs/Imu.h"
00045 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00046 #include "nav_msgs/Odometry.h"
00047 #include "std_msgs/Int16MultiArray.h"
00048 #include "std_msgs/MultiArrayLayout.h"
00049 #include "std_srvs/Empty.h"
00050
00051 #define MIP_SDK_GX4_45_IMU_STANDARD_MODE 0x01
00052 #define MIP_SDK_GX4_45_IMU_DIRECT_MODE 0x02
00053
00054 #define NUM_COMMAND_LINE_ARGUMENTS 3
00055
00056 #define DEFAULT_PACKET_TIMEOUT_MS 1000 //milliseconds
00057
00058
00059 #define Sleep(x) usleep(x*1000.0)
00060
00064 namespace Microstrain
00065 {
00070 class Microstrain
00071 {
00072 public:
00076 Microstrain();
00077
00079 ~Microstrain();
00080
00084 void run();
00085
00087 void filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00089 void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00091 void gps_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00092
00093 private:
00095 bool reset_callback(std_srvs::Empty::Request &req,
00096 std_srvs::Empty::Response &resp);
00098 void print_packet_stats();
00099
00100
00101
00102 mip_interface device_interface_;
00103
00104
00105 u32 filter_valid_packet_count_;
00106 u32 ahrs_valid_packet_count_;
00107 u32 gps_valid_packet_count_;
00108
00109 u32 filter_timeout_packet_count_;
00110 u32 ahrs_timeout_packet_count_;
00111 u32 gps_timeout_packet_count_;
00112
00113 u32 filter_checksum_error_packet_count_;
00114 u32 ahrs_checksum_error_packet_count_;
00115 u32 gps_checksum_error_packet_count_;
00116
00117
00118
00119 mip_ahrs_scaled_gyro curr_ahrs_gyro_;
00120 mip_ahrs_scaled_accel curr_ahrs_accel_;
00121 mip_ahrs_scaled_mag curr_ahrs_mag_;
00122 mip_ahrs_quaternion curr_ahrs_quaternion_;
00123
00124 mip_gps_llh_pos curr_llh_pos_;
00125 mip_gps_ned_vel curr_ned_vel_;
00126 mip_gps_time curr_gps_time_;
00127
00128
00129 mip_filter_llh_pos curr_filter_pos_;
00130 mip_filter_ned_velocity curr_filter_vel_;
00131 mip_filter_attitude_euler_angles curr_filter_angles_;
00132 mip_filter_attitude_quaternion curr_filter_quaternion_;
00133 mip_filter_compensated_angular_rate curr_filter_angular_rate_;
00134 mip_filter_llh_pos_uncertainty curr_filter_pos_uncertainty_;
00135 mip_filter_ned_vel_uncertainty curr_filter_vel_uncertainty_;
00136 mip_filter_euler_attitude_uncertainty curr_filter_att_uncertainty_;
00137 mip_filter_status curr_filter_status_;
00138
00139
00140 ros::Publisher gps_pub_;
00141 ros::Publisher imu_pub_;
00142 ros::Publisher nav_pub_;
00143 ros::Publisher nav_status_pub_;
00144 sensor_msgs::NavSatFix gps_msg_;
00145 sensor_msgs::Imu imu_msg_;
00146 nav_msgs::Odometry nav_msg_;
00147 std_msgs::Int16MultiArray nav_status_msg_;
00148 std::string gps_frame_id_;
00149 std::string imu_frame_id_;
00150 std::string odom_frame_id_;
00151 std::string odom_child_frame_id_;
00152 bool publish_gps_;
00153 bool publish_imu_;
00154 bool publish_odom_;
00155
00156
00157 int nav_rate_;
00158 int imu_rate_;
00159 int gps_rate_;
00160 };
00161
00162
00163
00164 #ifdef __cplusplus
00165 extern "C"
00166 #endif
00167 {
00168
00172 void filter_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00176 void ahrs_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00180 void gps_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type);
00181
00182 #ifdef __cplusplus
00183 }
00184 #endif
00185
00186 }
00187
00188 #endif // _MICROSTRAIN_3DM_GX5_45_H