Skip to content

Commit

Permalink
GX5-25 KF IMU publishing (#23)
Browse files Browse the repository at this point in the history 
* Added filter message publication from the GX5-25. Did a little more cleanup in the header file.

* Last little bit of cleanup to make roslint happy.

* Updated filtered IMU publish rate so that if we publish filtered IMU messages we take the higher of the two rates.

* Added in a check if we are publishing filtered data rate.

* Updated driver so that it publishes properly dependent upon which IMU data the user is after. AHRS streams need to be enabled for both cases.

* Updated ENU NED swap to match device labeling.

* Typo fix... :)

* Added in the tf2_geometry_msgs so that the toMsg function is available.

* Forgot to expose the parameter for the ENU -> NED swap previously. Done now.

* Made sure that in the original setting it defualts to the proper X & Y swapped with Z inverted.

* Fix bug in issue #26.

* Corrected launch file to be microstrain.
  • Loading branch information
@samkys @tonybaltovski
samkys authored and tonybaltovski committed Oct 7, 2019
1 parent a646328 commit a309187
Show file tree
Hide file tree
Showing 3 changed files with 459 additions and 241 deletions.
262 changes: 130 additions & 132 deletions include/microstrain_mips/microstrain_3dm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,12 @@ extern "C"
#include <unistd.h>
#include <time.h>


// ROS
#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include "geometry_msgs/Vector3.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Int8.h"
Expand Down Expand Up @@ -82,7 +82,6 @@ extern "C"
#include "microstrain_mips/SetMagAdaptiveVals.h"
#include "microstrain_mips/SetMagDipAdaptiveVals.h"


#define MIP_SDK_GX4_45_IMU_STANDARD_MODE 0x01
#define MIP_SDK_GX4_45_IMU_DIRECT_MODE 0x02

Expand All @@ -98,8 +97,6 @@ extern "C"
#define GX5_25_DEVICE "3DM-GX5-25"
#define GX5_15_DEVICE "3DM-GX5-15"



/**
* \brief Contains functions for micostrain driver
*/
Expand Down Expand Up @@ -240,134 +237,135 @@ namespace Microstrain


private:
//! @brief Reset KF service callback
bool reset_callback(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &resp);
//! @brief Convience for printing packet stats
void print_packet_stats();



// Variables/fields
//The primary device interface structure
mip_interface device_interface_;
base_device_info_field device_info;
u8 temp_string[20];

//Packet Counters (valid, timeout, and checksum errors)
u32 filter_valid_packet_count_;
u32 ahrs_valid_packet_count_;
u32 gps_valid_packet_count_;

u32 filter_timeout_packet_count_;
u32 ahrs_timeout_packet_count_;
u32 gps_timeout_packet_count_;

u32 filter_checksum_error_packet_count_;
u32 ahrs_checksum_error_packet_count_;
u32 gps_checksum_error_packet_count_;

//Data field storage
//AHRS
mip_ahrs_scaled_gyro curr_ahrs_gyro_;
mip_ahrs_scaled_accel curr_ahrs_accel_;
mip_ahrs_scaled_mag curr_ahrs_mag_;
mip_ahrs_quaternion curr_ahrs_quaternion_;
//GPS
mip_gps_llh_pos curr_llh_pos_;
mip_gps_ned_vel curr_ned_vel_;
mip_gps_time curr_gps_time_;

//FILTER
mip_filter_llh_pos curr_filter_pos_;
mip_filter_ned_velocity curr_filter_vel_;
mip_filter_attitude_euler_angles curr_filter_angles_;
mip_filter_attitude_quaternion curr_filter_quaternion_;
mip_filter_compensated_angular_rate curr_filter_angular_rate_;
mip_filter_llh_pos_uncertainty curr_filter_pos_uncertainty_;
mip_filter_ned_vel_uncertainty curr_filter_vel_uncertainty_;
mip_filter_euler_attitude_uncertainty curr_filter_att_uncertainty_;
mip_filter_status curr_filter_status_;

// ROS
ros::Publisher gps_pub_;
ros::Publisher imu_pub_;
ros::Publisher nav_pub_;
ros::Publisher nav_status_pub_;
ros::Publisher bias_pub_;
ros::Publisher device_status_pub_;
sensor_msgs::NavSatFix gps_msg_;
sensor_msgs::Imu imu_msg_;
nav_msgs::Odometry nav_msg_;
std_msgs::Int16MultiArray nav_status_msg_;
geometry_msgs::Vector3 bias_msg_;
std::string gps_frame_id_;
std::string imu_frame_id_;
std::string odom_frame_id_;
std::string odom_child_frame_id_;
microstrain_mips::status_msg device_status_msg_;
bool publish_gps_;
bool publish_imu_;
bool publish_odom_;
bool publish_bias_;
std::vector<double> imu_linear_cov_;
std::vector<double> imu_angular_cov_;
std::vector<double> imu_orientation_cov_;

//Device Flags
bool GX5_15;
bool GX5_25;
bool GX5_35;
bool GX5_45;
bool GQX_45;
bool RQX_45;
bool CXX_45;
bool CVX_10;
bool CVX_15;
bool CVX_25;





// Update rates
int nav_rate_;
int imu_rate_;
int gps_rate_;

clock_t start;
float field_data[3];
float soft_iron[9];
float soft_iron_readback[9];
float angles[3];
float heading_angle;
float readback_angles[3];
float noise[3];
float beta[3];
float readback_beta[3];
float readback_noise[3];
float offset[3];
float readback_offset[3];
u8 com_mode;
u16 duration;
u8 reference_position_enable_command;
u8 reference_position_enable_readback;
double reference_position_command[3];
double reference_position_readback[3];
u8 enable_flag;
u16 estimation_control;
u16 estimation_control_readback;
u8 dynamics_mode;
u8 readback_dynamics_mode;
gx4_25_basic_status_field basic_field;
gx4_25_diagnostic_device_status_field diagnostic_field;
gx4_45_basic_status_field basic_field_45;
gx4_45_diagnostic_device_status_field diagnostic_field_45;
mip_complementary_filter_settings comp_filter_command, comp_filter_readback;
mip_filter_accel_magnitude_error_adaptive_measurement_command accel_magnitude_error_command, accel_magnitude_error_readback;
mip_filter_magnetometer_magnitude_error_adaptive_measurement_command mag_magnitude_error_command, mag_magnitude_error_readback;
mip_filter_magnetometer_dip_angle_error_adaptive_measurement_command mag_dip_angle_error_command, mag_dip_angle_error_readback;
mip_filter_zero_update_command zero_update_control, zero_update_readback;
//! @brief Reset KF service callback
bool reset_callback(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &resp);
//! @brief Convience for printing packet stats
void print_packet_stats();

// Variables/fields
//The primary device interface structure
mip_interface device_interface_;
base_device_info_field device_info;
u8 temp_string[20];

//Packet Counters (valid, timeout, and checksum errors)
u32 filter_valid_packet_count_;
u32 ahrs_valid_packet_count_;
u32 gps_valid_packet_count_;

u32 filter_timeout_packet_count_;
u32 ahrs_timeout_packet_count_;
u32 gps_timeout_packet_count_;

u32 filter_checksum_error_packet_count_;
u32 ahrs_checksum_error_packet_count_;
u32 gps_checksum_error_packet_count_;

//Data field storage
//AHRS
mip_ahrs_scaled_gyro curr_ahrs_gyro_;
mip_ahrs_scaled_accel curr_ahrs_accel_;
mip_ahrs_scaled_mag curr_ahrs_mag_;
mip_ahrs_quaternion curr_ahrs_quaternion_;
//GPS
mip_gps_llh_pos curr_llh_pos_;
mip_gps_ned_vel curr_ned_vel_;
mip_gps_time curr_gps_time_;

//FILTER
mip_filter_llh_pos curr_filter_pos_;
mip_filter_ned_velocity curr_filter_vel_;
mip_filter_attitude_euler_angles curr_filter_angles_;
mip_filter_attitude_quaternion curr_filter_quaternion_;
mip_filter_compensated_angular_rate curr_filter_angular_rate_;
mip_filter_compensated_acceleration curr_filter_accel_comp_;
mip_filter_linear_acceleration curr_filter_linear_accel_;
mip_filter_llh_pos_uncertainty curr_filter_pos_uncertainty_;
mip_filter_ned_vel_uncertainty curr_filter_vel_uncertainty_;
mip_filter_euler_attitude_uncertainty curr_filter_att_uncertainty_;
mip_filter_status curr_filter_status_;

// ROS
ros::Publisher gps_pub_;
ros::Publisher imu_pub_;
ros::Publisher filtered_imu_pub_;
ros::Publisher nav_pub_;
ros::Publisher nav_status_pub_;
ros::Publisher bias_pub_;
ros::Publisher device_status_pub_;
sensor_msgs::NavSatFix gps_msg_;
sensor_msgs::Imu imu_msg_;
sensor_msgs::Imu filtered_imu_msg_;
nav_msgs::Odometry nav_msg_;
std_msgs::Int16MultiArray nav_status_msg_;
geometry_msgs::Vector3 bias_msg_;
std::string gps_frame_id_;
std::string imu_frame_id_;
std::string odom_frame_id_;
std::string odom_child_frame_id_;
microstrain_mips::status_msg device_status_msg_;
bool publish_gps_;
bool publish_imu_;
bool publish_odom_;
bool publish_bias_;
bool publish_filtered_imu_;
bool remove_imu_gravity_;
bool frame_based_enu_;
std::vector<double> imu_linear_cov_;
std::vector<double> imu_angular_cov_;
std::vector<double> imu_orientation_cov_;

//Device Flags
bool GX5_15;
bool GX5_25;
bool GX5_35;
bool GX5_45;
bool GQX_45;
bool RQX_45;
bool CXX_45;
bool CVX_10;
bool CVX_15;
bool CVX_25;

// Update rates
int nav_rate_;
int imu_rate_;
int gps_rate_;

clock_t start;
float field_data[3];
float soft_iron[9];
float soft_iron_readback[9];
float angles[3];
float heading_angle;
float readback_angles[3];
float noise[3];
float beta[3];
float readback_beta[3];
float readback_noise[3];
float offset[3];
float readback_offset[3];
u8 com_mode;
u16 duration;
u8 reference_position_enable_command;
u8 reference_position_enable_readback;
double reference_position_command[3];
double reference_position_readback[3];
u8 enable_flag;
u16 estimation_control;
u16 estimation_control_readback;
u8 dynamics_mode;
u8 readback_dynamics_mode;
gx4_25_basic_status_field basic_field;
gx4_25_diagnostic_device_status_field diagnostic_field;
gx4_45_basic_status_field basic_field_45;
gx4_45_diagnostic_device_status_field diagnostic_field_45;
mip_complementary_filter_settings comp_filter_command, comp_filter_readback;
mip_filter_accel_magnitude_error_adaptive_measurement_command accel_magnitude_error_command, accel_magnitude_error_readback;
mip_filter_magnetometer_magnitude_error_adaptive_measurement_command mag_magnitude_error_command, mag_magnitude_error_readback;
mip_filter_magnetometer_dip_angle_error_adaptive_measurement_command mag_dip_angle_error_command, mag_dip_angle_error_readback;
mip_filter_zero_update_command zero_update_control, zero_update_readback;
}; // Microstrain class


Expand Down
12 changes: 12 additions & 0 deletions launch/microstrain.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,12 @@
<param name="save_settings" value="true" type="bool" />
<param name="auto_init" value="true" type="bool" />

<!-- This parameter is to set wether the device orientation uses a basic
NED->ENU orientation swap or not. If true, the ENU reported should
match the label printed on the device. If false X & Y are swapped
and Z is negated. -->
<param name="frame_based_enu" value="false" type="bool" />

<!-- The GX5-25 is AHRS only, so need to turn off the other messages -->
<!-- AHRS Settings -->
<param name="publish_imu" value="true" type="bool" />
Expand All @@ -37,6 +43,12 @@
<!-- Declination source 1=None, 2=magnetic, 3=manual -->
<param name="declination_source" value="2" type="int" />
<param name="declination" value="0.23" type="double" />
<!-- Filtered IMU rate is based on nav_rate since it is tied in with the onboard Kalman Filter -->
<!-- If you set the filtered_imu rate to be something fairly high, make sure to lower the IMU rate
above since it appears that the data rate can flood the USB. -->
<param name="publish_filtered_imu" value="false" type="bool" />
<!-- Remove gravity is only valid with the filtered IMU data. -->
<param name="remove_imu_gravity" value="true" type="bool" />
<!-- Static IMU message covariance values -->
<!-- Since internally these are std::vector we need to use the rosparam tags -->
<rosparam param="imu_orientation_cov"> [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>
Expand Down
Loading

0 comments on commit a309187

Please sign in to comment.