Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enu/ned/swap #25

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
Open

Enu/ned/swap #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
20d25e0
Added filter message publication from the GX5-25. Did a little more c…
Jul 16, 2019
e201abb
Last little bit of cleanup to make roslint happy.
Jul 16, 2019
6d4c888
Updated filtered IMU publish rate so that if we publish filtered IMU …
Jul 25, 2019
64f636d
Added in a check if we are publishing filtered data rate.
Jul 25, 2019
d2ba7a6
Updated driver so that it publishes properly dependent upon which IMU…
Aug 15, 2019
df4cfe3
Updated ENU NED swap to match device labeling.
Aug 15, 2019
03398c8
Typo fix... :)
Aug 15, 2019
0002484
Added in the tf2_geometry_msgs so that the toMsg function is available.
Aug 15, 2019
7254cba
Forgot to expose the parameter for the ENU -> NED swap previously. Do…
Aug 15, 2019
98c79a9
Made sure that in the original setting it defualts to the proper X & …
Aug 15, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
262 changes: 130 additions & 132 deletions include/microstrain_mips/microstrain_3dm.h
View file
Open in desktop
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
14 changes: 13 additions & 1 deletion launch/microstrain.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<!-- Standalone example launch file for 3DM-GX5-25 -->

<!-- Declare arguments with default values -->
<arg name="port" default="/dev/microstrain" />
<arg name="port" default="/dev/ttyACM0" />
<arg name="baudrate" default="115200" />
<arg name="imu_rate" default="100" />
<arg name="imu_frame_id" default="gx5_link" />
Expand All @@ -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