diff --git a/README.md b/README.md index b0022810..fda9ce93 100644 --- a/README.md +++ b/README.md @@ -6,3 +6,5 @@ Interface software, including ROS node, for Microstrain 3DM-GX5-45. See http://wiki.ros.org/microstrain_3dm_gx5_45 + +Modification from wiki as it runs the Microstrain 3dm gx5 25. diff --git a/launch/microstrain_25.launch b/launch/microstrain_25.launch index 6a2846d9..fbbc5fb5 100644 --- a/launch/microstrain_25.launch +++ b/launch/microstrain_25.launch @@ -7,34 +7,7 @@ pkg="microstrain_3dm_gx5_45" type="microstrain_3dm_gx5_25_node" output="screen"> - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/params/gx5_25.yaml b/params/gx5_25.yaml new file mode 100644 index 00000000..a6d28e94 --- /dev/null +++ b/params/gx5_25.yaml @@ -0,0 +1,49 @@ +port: /dev/ttyACM0 +baudrate: 115200 +device_setup: true +readback_settings: true +save_settings: false + +# Orientation settings +tf_ned_to_enu: true +publish_rpy: false +publish_mag: false + +# Complementary filter settings +publish_cf: false +cf_rate: 100 + +# EKF settings +auto_init: true +imu_frame_id: imu_link +ekf_rate: 100 + +# Dynamics mode 1 = Portable, 2 = Automotive, 3 = Airborne +dynamics_mode: 1 + +# Declination source +# If using 2 make sure to set the lat long data for the world model. +# Declination source 1=None, 2=magnetic, 3=manual +declination_source: 3 + +# Declination value, set to your magnetic declination +declination: -0.072139694 + +# Make sure all covariances below are of type xx.xx , i.e. double so that the rpc is parsed correctly + +# Linear Acceleration Covariances not produced by the sensor +linear_accel_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + +# Angular Velocity Covariances not produced by the sensor +angular_vel_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + +# Complementary filter orientation covariance, actual covariances output from +# sensor EKF when set to publish in that mode +cf_orientation_covariance: [0.01, 0.0, 0.0, + 0.0, 0.01, 0.0, + 0.0, 0.0, 0.01] + diff --git a/src/microstrain_3dm_gx5_25.cpp b/src/microstrain_3dm_gx5_25.cpp index 17180c7e..70435643 100644 --- a/src/microstrain_3dm_gx5_25.cpp +++ b/src/microstrain_3dm_gx5_25.cpp @@ -25,8 +25,22 @@ #include #include +//Unused covariances initilized to zero's +boost::array linear_accel_covariance = { }; +boost::array angular_vel_covariance = { }; +boost::array cf_orientation_covariance = { }; +XmlRpc::XmlRpcValue rpc_temp; + +// Temporary vector used to read from param server +std::vector temp; + +// Boolean used so we don't publish until all is initilized +bool initilized = false; +bool solution_valid = false; //<-- If the EKF solution is not valid, don't publish + namespace Microstrain { + Microstrain::Microstrain() : // Initialization list filter_valid_packet_count_(0), @@ -41,20 +55,39 @@ Microstrain::Microstrain() : publish_mag_(false), tf_ned_to_enu_(true) { - // pass + // pass } + Microstrain::~Microstrain() { // pass } + + +//Basic loop so we can initilize our covariance parameters above +boost::array setCov(XmlRpc::XmlRpcValue rpc){ + //Output covariance vector + boost::array output = { 0.0 }; + + //Convert the RPC message to array + ROS_ASSERT(rpc.getType() == XmlRpc::XmlRpcValue::TypeArray); + + for(int i = 0; i < 9; i++){ + ROS_ASSERT(rpc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); + output[i] = (double)rpc[i]; + } + return output; +} + + //void quaternionCovFromYawPitchRollCov(float yaw, float pitch, float roll); void Microstrain::run() { -// Variables for device configuration, ROS parameters, etc. + // Variables for device configuration, ROS parameters, etc. u32 com_port, baudrate; bool device_setup = false; - bool readback_settings = true; - bool save_settings = true; + bool readback_settings = true; + bool save_settings = true; bool auto_init = true; u8 auto_init_u8 = 1; u8 readback_headingsource = 0; @@ -99,6 +132,8 @@ void Microstrain::run() float readback_noise[3] = {0}; float beta[3] = {0}; float readback_beta[3] = {0}; + + // Mip low pass filter setting defined in------>microstrain_3dm_gx5_45-25-dev/MIPSDK/C/Library/Include/mip_sdk_ahrs.h mip_low_pass_filter_settings filter_settings; float bias_vector[3] = {0}; u16 duration = 0; @@ -111,22 +146,26 @@ void Microstrain::run() mip_filter_external_heading_with_time_command external_heading_with_time; 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_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; // ROS setup ros::Time::init(); ros::NodeHandle node; ros::NodeHandle private_nh("~"); - // ROS Parameters + // ROS Parameters // Comms Parameters std::string port; int baud, pdyn_mode; private_nh.param("port", port, std::string("/dev/ttyACM0")); private_nh.param("baudrate",baud,115200); baudrate = (u32)baud; + // Configuration Parameters private_nh.param("device_setup",device_setup,false); private_nh.param("readback_settings",readback_settings,true); @@ -135,31 +174,46 @@ void Microstrain::run() private_nh.param("auto_init",auto_init,true); private_nh.param("cf_rate",cf_rate_, 10); private_nh.param("ekf_rate",ekf_rate_, 10); - private_nh.param("dynamics_mode",pdyn_mode,1); + private_nh.param("dynamics_mode",pdyn_mode,1); + + //setting our linear covariances from the parameter server + if(private_nh.getParam("linear_accel_covariance",rpc_temp)) + { + linear_accel_covariance = setCov(rpc_temp); + } + if(private_nh.getParam("angular_vel_covariance",rpc_temp)) + { + angular_vel_covariance = setCov(rpc_temp); + } + if(private_nh.getParam("cf_orientation_covariance",rpc_temp)) + { + cf_orientation_covariance = setCov(rpc_temp); + } /////////////////////////////////////////////////////////////////////// //Dynamics Mode Options: //1 = Portable, 2 = Automotive, 3 = Airborne /////////////////////////////////////////////////////////////////////// - dynamics_mode = (u8)pdyn_mode; //(u8)pdyn_mode; + dynamics_mode = (u8)pdyn_mode; //(u8)pdyn_mode; if (dynamics_mode < 1 || dynamics_mode > 3) { ROS_WARN("dynamics_mode can't be %#04X, must be 1, 2 or 3. Setting to 1.",dynamics_mode); - dynamics_mode = 1; + dynamics_mode = 1; } /////////////////////////////////////////////////////////////////////// //Declination Source (0x0D, 0x43) //0x01 - None, 0x02 - World Magnetic Model (Default), 0x03 (Manual) - /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// ROS_INFO("Dynamics mode set to: %#04X", dynamics_mode); private_nh.param("declination_source",declination_source,2); - if (declination_source < 1 || declination_source > 3) { + if (declination_source < 1 || declination_source > 3) + { ROS_WARN("declination_source can't be %#04X, must be 1, 2 or 3. Setting to 2.",declination_source); declination_source = 2; } declination_source_u8 = (u8)declination_source; - private_nh.param("declination",declination,0.23); + private_nh.param("declination",declination,0.23); private_nh.param("imu_frame_id",imu_frame_id_, std::string("imu_link")); private_nh.param("publish_cf",publish_cf_, false); private_nh.param("publish_rpy",publish_rpy_,false); @@ -170,45 +224,66 @@ void Microstrain::run() status_pub_ = node.advertise("imu/status",100); ekf_pub_ = node.advertise("imu/data",100); if (publish_rpy_) - ekf_rpy_pub_ = node.advertise("imu/rpy",100); + { + ekf_rpy_pub_ = node.advertise("imu/rpy",100); + } if (publish_cf_) { cf_pub_ = node.advertise("imu/cf/data",100); if (publish_rpy_) - cf_rpy_pub_ = node.advertise("imu/cf/rpy",100); + { + cf_rpy_pub_ = node.advertise("imu/cf/rpy",100); + } } if (publish_mag_) - mag_pub_ = node.advertise("imu/mag",100); //SJP Added - + { + mag_pub_ = node.advertise("imu/mag",100); //SJP Added + } + // Kalman filter reset callback ros::ServiceServer service = node.advertiseService("reset_kf", &Microstrain::reset_callback, this); - //Initialize the serial interface to the device - ROS_INFO("Attempting to open serial port <%s> at <%d> \n", - port.c_str(),baudrate); - if(mip_interface_init(port.c_str(), baudrate, &device_interface_, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK) { + ROS_INFO("Attempting to open serial port <%s> at <%d> \n", port.c_str(),baudrate); + if(mip_interface_init(port.c_str(), + baudrate, + &device_interface_, + DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK) + { ROS_FATAL("Couldn't open serial port! Is it plugged in?"); } - - // Setup device callbacks - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_FILTER_DATA_SET, this, &filter_packet_callback_wrapper) != MIP_INTERFACE_OK) - { - ROS_INFO("Can't setup filter (EKF Estimate) callback!"); + + /*---------------------------------*/ + /* Setup device callback functions */ + /*---------------------------------*/ + + //EKF estimation callback + if(mip_interface_add_descriptor_set_callback(&device_interface_, + MIP_FILTER_DATA_SET, + this, + &filter_packet_callback_wrapper) != MIP_INTERFACE_OK) + { + ROS_INFO("Can't setup filter (EKF Estimate) callback!"); return; } - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET, this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) + + //Complementary filter callback + if(mip_interface_add_descriptor_set_callback(&device_interface_, + MIP_AHRS_DATA_SET, + this, + &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) { ROS_INFO("Can't setup ahrs (CF Estimate) callbacks!"); return; } - //////////////////////////////////////// - // Device setup - //////////////////////////////////////// + /*---------------------------------*/ + /* Device setup */ + /*---------------------------------*/ + float dT=0.5; // common sleep time after setup communications if (device_setup) { @@ -216,13 +291,19 @@ void Microstrain::run() ROS_INFO("Putting device communications into 'standard mode'"); device_descriptors_size = 128*2; com_mode = MIP_SDK_GX4_25_IMU_STANDARD_MODE; //MIP_SDK_GX4_45_IMU_STANDARD_MODE = 0x01 - while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK) {} + while(mip_system_com_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &com_mode) != MIP_INTERFACE_OK) {} + //Verify device mode setting ROS_INFO("Verify comm's mode"); - while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK) {} + while(mip_system_com_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + &com_mode) != MIP_INTERFACE_OK) {} ROS_INFO("Sleep for a second..."); ros::Duration(dT).sleep(); ROS_INFO("Right mode?"); + if(com_mode != MIP_SDK_GX4_25_IMU_STANDARD_MODE) { ROS_ERROR("Appears we didn't get into standard mode!"); @@ -233,224 +314,266 @@ void Microstrain::run() while(mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); + /*------------------------------------*/ + /* Magnetometer calibration readbacks */ + /*------------------------------------*/ - ////////////////////////////////////////////////////////////////////////// - // Magnetometer calibration readbacks - ////////////////////////////////////////////////////////////////////////// - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, hard_iron_readback) != MIP_INTERFACE_OK){} + while(mip_3dm_cmd_hard_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + hard_iron_readback) != MIP_INTERFACE_OK){} std::cout << "Hard iron offsets are: [X = " << hard_iron_readback[0] << - "] [Y = " << hard_iron_readback[1] << - "] [Z = " << hard_iron_readback[2] << "]." << std::endl; - - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){} - std::cout << "Soft iron matrix is: [" << soft_iron_readback[0] << " " << soft_iron_readback[1] << " " << soft_iron_readback[2] << std::endl; - std::cout << " " << soft_iron_readback[3] << " " << soft_iron_readback[4] << " " << soft_iron_readback[5] << std::endl; - std::cout << " " << soft_iron_readback[6] << " " << soft_iron_readback[7] << " " << soft_iron_readback[8] << "]." << std::endl; - - //////////////////////////////////// - // AHRS Setup - Complementary Filter - //////////////////////////////////// - if (publish_cf_ | publish_mag_) { - // Get base rate - while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) {} - ROS_INFO("AHRS Base Rate => %d Hz", base_rate); - ros::Duration(dT).sleep(); - - // Deterimine decimation to get close to goal rate - u8 cf_decimation = (u8)((float)base_rate/ (float)cf_rate_); - ROS_INFO("AHRS (CF) decimation set to %#04X",cf_decimation); - - // AHRS Message Format - ROS_INFO("Setting the AHRS message format"); - data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED; - data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED; - data_stream_format_descriptors[2] = MIP_AHRS_DATA_QUATERNION; - data_stream_format_descriptors[3] = MIP_AHRS_DATA_MAG_SCALED; - data_stream_format_descriptors[4] = MIP_AHRS_DATA_EULER_ANGLES; - - data_stream_format_decimation[0] = cf_decimation; //0x32; - data_stream_format_decimation[1] = cf_decimation; //0x32; - data_stream_format_decimation[2] = cf_decimation; //0x32; //SJP Added - data_stream_format_decimation[3] = cf_decimation; //0x32; SJP Added //SJP Added - data_stream_format_decimation[4] = cf_decimation; //0x32; SJP Added + "] [Y = " << hard_iron_readback[1] << + "] [Z = " << hard_iron_readback[2] << "]." << std::endl; + + while(mip_3dm_cmd_soft_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + soft_iron_readback) != MIP_INTERFACE_OK){} + std::cout << "Soft iron matrix is: [" << soft_iron_readback[0] << + " " << soft_iron_readback[1] << + " " << soft_iron_readback[2] << std::endl; + std::cout << " " << soft_iron_readback[3] << + " " << soft_iron_readback[4] << + " " << soft_iron_readback[5] << std::endl; + std::cout << " " << soft_iron_readback[6] << + " " << soft_iron_readback[7] << + " " << soft_iron_readback[8] << "]." << std::endl; + + /*-----------------------------------*/ + /* AHRS Setup - Complementary Filter */ + /*-----------------------------------*/ + + if (publish_cf_ | publish_mag_) { + // Get base rate + while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, + &base_rate) != MIP_INTERFACE_OK) {} + ROS_INFO("AHRS Base Rate => %d Hz", base_rate); + ros::Duration(dT).sleep(); + + // Deterimine decimation to get close to goal rate + u8 cf_decimation = (u8)((float)base_rate/ (float)cf_rate_); + ROS_INFO("AHRS (CF) decimation set to %#04X",cf_decimation); + + // AHRS Message Format + ROS_INFO("Setting the AHRS message format"); + data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED; + data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED; + data_stream_format_descriptors[2] = MIP_AHRS_DATA_QUATERNION; + data_stream_format_descriptors[3] = MIP_AHRS_DATA_MAG_SCALED; + data_stream_format_descriptors[4] = MIP_AHRS_DATA_EULER_ANGLES; + + data_stream_format_decimation[0] = cf_decimation;//0x32; + data_stream_format_decimation[1] = cf_decimation;//0x32; + data_stream_format_decimation[2] = cf_decimation;//0x32;//SJP Added + data_stream_format_decimation[3] = cf_decimation; //0x32; SJP Added//SJP Added + data_stream_format_decimation[4] = cf_decimation;//0x32; SJP Added + + data_stream_format_num_entries = 5;//SJP Changed from 3; + + + + // Set message format + while(mip_3dm_cmd_ahrs_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &data_stream_format_num_entries, + data_stream_format_descriptors, + data_stream_format_decimation) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + + // Poll to verify + ROS_INFO("Poll AHRS data to verify"); + while(mip_3dm_cmd_poll_ahrs(&device_interface_, + MIP_3DM_POLLING_ENABLE_ACK_NACK, + data_stream_format_num_entries, + data_stream_format_descriptors) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + + // Save + if (save_settings) + { + ROS_INFO("Saving AHRS (CF) data settings"); + while(mip_3dm_cmd_ahrs_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, + 0, NULL,NULL) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + } + + // Declination Source + // Set declination + ROS_INFO("Setting declination source to %#04X",declination_source_u8); + while(mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &declination_source_u8) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + + //Read back the declination source + ROS_INFO("Reading back declination source"); + while(mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + &readback_declination_source) != MIP_INTERFACE_OK) {} + + if(declination_source_u8 == readback_declination_source) + { + ROS_INFO("Success: Declination source set to %#04X", declination_source_u8); + } + else + { + ROS_WARN("Failed to set the declination source to %#04X!", declination_source_u8); + } + ros::Duration(dT).sleep(); + + if (save_settings) + { + ROS_INFO("Saving declination source settings to EEPROM"); + while(mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, + NULL) != MIP_INTERFACE_OK){} + ros::Duration(dT).sleep(); + } + + } // end of AHRS setup + + + + /*-----------------------------------*/ + /* Filter Setup - EKF Estimate */ + /*-----------------------------------*/ + //Get base rate + while(mip_3dm_cmd_get_filter_base_rate(&device_interface_, + &base_rate) != MIP_INTERFACE_OK) {} + ROS_INFO("Filter (EKF) Base Rate => %d Hz", base_rate); + u8 ekf_decimation = (u8)((float)base_rate/ (float)ekf_rate_); + ros::Duration(dT).sleep(); + ROS_INFO("Filter (EKF) decimation set to %#04X",ekf_decimation); + + //Filter Message Format + ROS_INFO("Setting Filter stream format"); + data_stream_format_descriptors[0] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION; + data_stream_format_descriptors[1] = MIP_FILTER_DATA_ATT_EULER_ANGLES; + data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; + data_stream_format_descriptors[3] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; + data_stream_format_descriptors[4] = MIP_FILTER_DATA_FILTER_STATUS; + data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; + data_stream_format_descriptors[6] = MIP_FILTER_DATA_ATT_UNCERTAINTY_QUATERNION; + data_stream_format_descriptors[7] = MIP_FILTER_DATA_WGS84_GRAVITY; //Local gravity + + data_stream_format_decimation[0] = ekf_decimation;//0x32; + data_stream_format_decimation[1] = ekf_decimation;//0x32; + data_stream_format_decimation[2] = ekf_decimation;//0x32; + data_stream_format_decimation[3] = ekf_decimation;//0x32; + data_stream_format_decimation[4] = ekf_decimation;//0x32; + data_stream_format_decimation[5] = ekf_decimation; + data_stream_format_decimation[6] = ekf_decimation; + data_stream_format_decimation[7] = ekf_decimation; + + data_stream_format_num_entries = 8; + + ROS_INFO("Finished setting Filter stream format"); + + // Set Filter Message Format + while(mip_3dm_cmd_filter_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &data_stream_format_num_entries, + data_stream_format_descriptors, + data_stream_format_decimation) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); - data_stream_format_num_entries = 5; //SJP Changed from 3; + // Poll to verify + ROS_INFO("Poll filter data to test stream"); + while(mip_3dm_cmd_poll_filter(&device_interface_, + MIP_3DM_POLLING_ENABLE_ACK_NACK, + data_stream_format_num_entries, + data_stream_format_descriptors) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); - // Set message format - while(mip_3dm_cmd_ahrs_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries, data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) {} + // Save + if (save_settings) + { + ROS_INFO("Saving Filter data settings"); + while(mip_3dm_cmd_filter_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, + 0, NULL,NULL) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); + } - // Poll to verify - ROS_INFO("Poll AHRS data to verify"); - while(mip_3dm_cmd_poll_ahrs(&device_interface_, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Dynamics Mode + // Set dynamics mode + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /* + ROS_INFO("Setting dynamics mode to %#04X",dynamics_mode); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &dynamics_mode) != MIP_INTERFACE_OK){} + ros::Duration(dT).sleep(); - // Save - if (save_settings) - { - ROS_INFO("Saving AHRS (CF) data settings"); - while(mip_3dm_cmd_ahrs_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); - } - - // Declination Source - // Set declination - ROS_INFO("Setting declination source to %#04X",declination_source_u8); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK) {} + // Readback dynamics mode + if (readback_settings) + { + // Read the settings back + ROS_INFO("Reading back dynamics mode setting"); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + &readback_dynamics_mode)!= MIP_INTERFACE_OK){} ros::Duration(dT).sleep(); - //Read back the declination source - ROS_INFO("Reading back declination source"); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK) {} - if(declination_source_u8 == readback_declination_source) - { - ROS_INFO("Success: Declination source set to %#04X", declination_source_u8); - } + if (dynamics_mode == readback_dynamics_mode) + { + ROS_INFO("Success: Dynamics mode setting is: %#04X",readback_dynamics_mode); + } else - { - ROS_WARN("Failed to set the declination source to %#04X!", declination_source_u8); - } - ros::Duration(dT).sleep(); - if (save_settings) - { - ROS_INFO("Saving declination source settings to EEPROM"); - while(mip_filter_declination_source(&device_interface_, - MIP_FUNCTION_SELECTOR_STORE_EEPROM, - NULL) != MIP_INTERFACE_OK) - {} - ros::Duration(dT).sleep(); - } - - } // end of AHRS setup - - - ////////////////////////////// - // Filter Setup - EKF Estimate - ////////////////////////////// - //Get base rate - while(mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) {} - ROS_INFO("Filter (EKF) Base Rate => %d Hz", base_rate); - u8 ekf_decimation = (u8)((float)base_rate/ (float)ekf_rate_); - ros::Duration(dT).sleep(); - ROS_INFO("Filter (EKF) decimation set to %#04X",ekf_decimation); - - //Filter Message Format - ROS_INFO("Setting Filter stream format"); - data_stream_format_descriptors[0] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION; - data_stream_format_descriptors[1] = MIP_FILTER_DATA_ATT_EULER_ANGLES; - data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; - data_stream_format_descriptors[3] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; - data_stream_format_descriptors[4] = MIP_FILTER_DATA_FILTER_STATUS; - data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; - data_stream_format_descriptors[6] = MIP_FILTER_DATA_ATT_UNCERTAINTY_QUATERNION; - - data_stream_format_decimation[0] = ekf_decimation; //0x32; - data_stream_format_decimation[1] = ekf_decimation; //0x32; - data_stream_format_decimation[2] = ekf_decimation; //0x32; - data_stream_format_decimation[3] = ekf_decimation; //0x32; - data_stream_format_decimation[4] = ekf_decimation; //0x32; - data_stream_format_decimation[5] = ekf_decimation; - data_stream_format_decimation[6] = ekf_decimation; - - data_stream_format_num_entries = 7; - - ROS_INFO("Finished setting Filter stream format"); - - // Set Filter Message Format - while(mip_3dm_cmd_filter_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); - - // Poll to verify - ROS_INFO("Poll filter data to test stream"); - while(mip_3dm_cmd_poll_filter(&device_interface_, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); + { + ROS_ERROR("Failure: Dynamics mode set to be %#04X, but reads as %#04X", + dynamics_mode,readback_dynamics_mode); + } + } - // Save - if (save_settings) - { - ROS_INFO("Saving Filter data settings"); - while(mip_3dm_cmd_filter_message_format(&device_interface_, - MIP_FUNCTION_SELECTOR_STORE_EEPROM, - 0, NULL,NULL) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); - } - - - /////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////// - // Dynamics Mode - // Set dynamics mode - /////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////// - /* - ROS_INFO("Setting dynamics mode to %#04X",dynamics_mode); + if (save_settings) + { + ROS_INFO("Saving dynamics mode settings to EEPROM"); while(mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_WRITE, - &dynamics_mode) != MIP_INTERFACE_OK){} + MIP_FUNCTION_SELECTOR_STORE_EEPROM, + NULL) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); + } + */ - // Readback dynamics mode - if (readback_settings) - { - // Read the settings back - ROS_INFO("Reading back dynamics mode setting"); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_READ, - &readback_dynamics_mode)!= MIP_INTERFACE_OK){} - ros::Duration(dT).sleep(); - if (dynamics_mode == readback_dynamics_mode) - ROS_INFO("Success: Dynamics mode setting is: %#04X",readback_dynamics_mode); - else - ROS_ERROR("Failure: Dynamics mode set to be %#04X, but reads as %#04X", - dynamics_mode,readback_dynamics_mode); - } - - if (save_settings) - { - ROS_INFO("Saving dynamics mode settings to EEPROM"); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, - MIP_FUNCTION_SELECTOR_STORE_EEPROM, - NULL) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); - } - */ - - - /////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////// - // Heading Source -- Select the source for aiding heading - // updates to the Kalman filter - // 0 = No heading aids, 1 = Use the internal magnetometer, 3 = Use external heading message - /////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////// - - ROS_INFO("Set heading source to internal mag."); - heading_source = 0x1; - - // Set heading source - ROS_INFO("Setting heading source to %#04X",heading_source); - while(mip_filter_heading_source(&device_interface_, - MIP_FUNCTION_SELECTOR_WRITE, - &heading_source) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Heading Source -- Select the source for aiding heading + // updates to the Kalman filter + // 0 = No heading aids, 1 = Use the internal magnetometer, 3 = Use external heading message + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// - // Read heading source - ROS_INFO("Read back heading source..."); - while(mip_filter_heading_source(&device_interface_, - MIP_FUNCTION_SELECTOR_READ, - &readback_headingsource)!= MIP_INTERFACE_OK) {} - ROS_INFO("Heading source = %#04X",readback_headingsource); - ros::Duration(dT).sleep(); + ROS_INFO("Set heading source to internal mag."); + heading_source = 0x1; - // Save heading source - if (save_settings) - { - ROS_INFO("Saving heading source to EEPROM"); - while(mip_filter_heading_source(&device_interface_, - MIP_FUNCTION_SELECTOR_STORE_EEPROM, - NULL)!= MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); - } + // Set heading source + ROS_INFO("Setting heading source to %#04X",heading_source); + while(mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + &heading_source) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + + // Read heading source + ROS_INFO("Read back heading source..."); + while(mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, + &readback_headingsource)!= MIP_INTERFACE_OK) {} + ROS_INFO("Heading source = %#04X",readback_headingsource); + ros::Duration(dT).sleep(); + + // Save heading source + if (save_settings) + { + ROS_INFO("Saving heading source to EEPROM"); + while(mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, + NULL)!= MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); + } // end of Filter setup @@ -466,8 +589,7 @@ void Microstrain::run() auto_init_u8 = auto_init; // convert bool to u8 while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, - &auto_init_u8) != MIP_INTERFACE_OK) - {} + &auto_init_u8) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); // Read initialization settings @@ -477,14 +599,17 @@ void Microstrain::run() ROS_INFO("Reading back auto-initialization value"); while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_READ, - &readback_auto_init)!= MIP_INTERFACE_OK) - {} + &readback_auto_init)!= MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); if (auto_init == readback_auto_init) + { ROS_INFO("Success: Auto init. setting is: %#04X",readback_auto_init); + } else + { ROS_ERROR("Failure: Auto init. setting set to be %#04X, but reads as %#04X", auto_init,readback_auto_init); + } } // Save initialization settings @@ -493,8 +618,7 @@ void Microstrain::run() ROS_INFO("Saving auto init. settings to EEPROM"); while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, - NULL) != MIP_INTERFACE_OK) - {} + NULL) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); } @@ -502,15 +626,21 @@ void Microstrain::run() // Enable Data streams ////////////////////// dT = 0.25; - ROS_INFO("Enabling Filter (EKF Estimate) stream"); - enable = 0x01; - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK) {} - ros::Duration(dT).sleep(); + ROS_INFO("Enabling Filter (EKF Estimate) stream"); + enable = 0x01; + while(mip_3dm_cmd_continuous_data_stream(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + MIP_3DM_INS_DATASTREAM, + &enable) != MIP_INTERFACE_OK) {} + ros::Duration(dT).sleep(); if (publish_cf_ | publish_mag_) { ROS_INFO("Enabling AHRS (Complementary Filter) stream"); enable = 0x01; - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK) {} + while(mip_3dm_cmd_continuous_data_stream(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, + MIP_3DM_AHRS_DATASTREAM, + &enable) != MIP_INTERFACE_OK) {} ros::Duration(dT).sleep(); } @@ -536,20 +666,27 @@ void Microstrain::run() max_rate = std::max(max_rate,cf_rate_); } - int spin_rate = std::min(3*max_rate,1000); ROS_INFO("Setting spin rate to <%d>",spin_rate); - ros::Rate r(spin_rate); // Rate in Hz + ros::Rate r(spin_rate);// Rate in Hz + + //Update the parser (this function reads the port and parses the bytes + mip_interface_update(&device_interface_); + ros::spinOnce();// Spin once to clear the buffer + r.sleep(); + // Device is initilized, we can set publishing to true + initilized = true; + ros::Duration(dT).sleep(); + while (ros::ok()) { //Update the parser (this function reads the port and parses the bytes mip_interface_update(&device_interface_); - ros::spinOnce(); // take care of service requests. + ros::spinOnce();// take care of service requests. r.sleep(); - //ROS_INFO("Spinning"); - } // end loop + }// end loop // close serial port mip_sdk_port_close(device_interface_.port_handle); @@ -557,6 +694,10 @@ void Microstrain::run() } // End of ::run() +/*-----------------------------------*/ +/* Filter reset - Reset Callback */ +/*-----------------------------------*/ + bool Microstrain::reset_callback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) { @@ -566,240 +707,290 @@ bool Microstrain::reset_callback(std_srvs::Empty::Request &req, return true; } + +/*-----------------------------------*/ +/* Filter Callback */ +/*-----------------------------------*/ + void Microstrain::filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type) { - //std::cout << "filter callback" << std::endl; + + //std::cout << "filter callback" << std::endl; mip_field_header *field_header; u8 *field_data; u16 field_offset = 0; // If we aren't publishing, then return - //The packet callback can have several types, process them all switch(callback_type) { - /// - //Handle valid packets - /// - - case MIP_INTERFACE_CALLBACK_VALID_PACKET: - { - filter_valid_packet_count_++; - - /// - //Loop through all of the data fields - /// - - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) - { - - /// - // Decode the field - /// - switch(field_header->descriptor) - { - ////////////////////////////////////////////////////////////// - // Accelerations - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_COMPENSATED_ACCELERATION: - { - memcpy(&curr_filter_accel_, field_data, sizeof(mip_filter_compensated_acceleration)); - - //For little-endian targets, byteswap the data field - mip_filter_compensated_acceleration_byteswap(&curr_filter_accel_); - - // Stuff into ROS message - acceleration in m/s^2 - // Header - ekf_imu_msg_.header.seq = filter_valid_packet_count_; - ekf_imu_msg_.header.stamp = ros::Time::now(); - ekf_imu_msg_.header.frame_id = imu_frame_id_; - if (tf_ned_to_enu_) - { - ekf_imu_msg_.linear_acceleration.x = curr_filter_accel_.y; - ekf_imu_msg_.linear_acceleration.y = curr_filter_accel_.x; - ekf_imu_msg_.linear_acceleration.z = -curr_filter_accel_.z; - } - else - { - ekf_imu_msg_.linear_acceleration.x = curr_filter_accel_.x; - ekf_imu_msg_.linear_acceleration.y = curr_filter_accel_.y; - ekf_imu_msg_.linear_acceleration.z = curr_filter_accel_.z; - } - ekf_rpy_msg_.header.seq = filter_valid_packet_count_; - ekf_rpy_msg_.header.stamp = ros::Time::now(); - ekf_rpy_msg_.header.frame_id = imu_frame_id_; - - } break; - - ////////////////////////////////////////////////////////////// - // Estimated Attitude, Euler Angles - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_ATT_EULER_ANGLES: - { - memcpy(&curr_filter_angles_, field_data, sizeof(mip_filter_attitude_euler_angles)); - - //For little-endian targets, byteswap the data field - mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_); - - if (tf_ned_to_enu_) - { - ekf_rpy_msg_.vector.x = curr_filter_angles_.pitch;//Roll in ENU = Pitch in NED - ekf_rpy_msg_.vector.y = curr_filter_angles_.roll; //Pitch in ENU = Roll in NED - ekf_rpy_msg_.vector.z = -curr_filter_angles_.yaw; //Yaw in ENU = -Yaw in NED - } - else - { - ekf_rpy_msg_.vector.x = curr_filter_angles_.roll; - ekf_rpy_msg_.vector.y = curr_filter_angles_.pitch; - ekf_rpy_msg_.vector.z = curr_filter_angles_.yaw; - } - } break; - - ////////////////////////////////////////////////////////////// - // Quaternion - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_ATT_QUATERNION: - { - memcpy(&curr_filter_quaternion_, field_data, sizeof(mip_filter_attitude_quaternion)); - - //For little-endian targets, byteswap the data field - mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); - - if (tf_ned_to_enu_) - { - ekf_imu_msg_.orientation.x = curr_filter_quaternion_.q[2]; - ekf_imu_msg_.orientation.y = curr_filter_quaternion_.q[1]; - ekf_imu_msg_.orientation.z = -1.0*curr_filter_quaternion_.q[3]; - ekf_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; - } - else - { - ekf_imu_msg_.orientation.x = curr_filter_quaternion_.q[1]; - ekf_imu_msg_.orientation.y = curr_filter_quaternion_.q[2]; - ekf_imu_msg_.orientation.z = curr_filter_quaternion_.q[3]; - ekf_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; - } - - } break; - - ////////////////////////////////////////////////////////////// - // Angular Rates - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE: - { - memcpy(&curr_filter_angular_rate_, field_data, sizeof(mip_filter_compensated_angular_rate)); - - //For little-endian targets, byteswap the data field - mip_filter_compensated_angular_rate_byteswap(&curr_filter_angular_rate_); - - if (tf_ned_to_enu_) - { - ekf_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.y; - ekf_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.x; - ekf_imu_msg_.angular_velocity.z = -curr_filter_angular_rate_.z; - } - else - { - ekf_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.x; - ekf_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.y; - ekf_imu_msg_.angular_velocity.z = curr_filter_angular_rate_.z; - } - - } break; - - ////////////////////////////////////////////////////////////// - // Attitude Uncertainty (Euler) - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER: - { - memcpy(&curr_filter_att_uncertainty_, field_data, sizeof(mip_filter_euler_attitude_uncertainty)); - - //For little-endian targets, byteswap the data field - mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_); - if (tf_ned_to_enu_) - { - ekf_imu_msg_.orientation_covariance[0] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; - ekf_imu_msg_.orientation_covariance[4] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; - ekf_imu_msg_.orientation_covariance[8] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; - } - else - { - ekf_imu_msg_.orientation_covariance[0] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; - ekf_imu_msg_.orientation_covariance[4] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; - ekf_imu_msg_.orientation_covariance[8] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; - } - } break; - - - ////////////////////////////////////////////////////////////// - // Attitude Uncertainty (Quaternion) - ////////////////////////////////////////////////////////////// - case MIP_FILTER_DATA_ATT_UNCERTAINTY_QUATERNION: - { - memcpy(&curr_filter_quat_uncertainty_, field_data, sizeof(mip_filter_quaternion_attitude_uncertainty)); - - //For little-endian targets, byteswap the data field - mip_filter_quaternion_attitude_uncertainty_byteswap(&curr_filter_quat_uncertainty_); - - //ekf_imu_msg_.orientation_covariance[1] = curr_filter_quat_uncertainty_.q2*curr_filter_quat_uncertainty_.q2; - //ekf_imu_msg_.orientation_covariance[5] = curr_filter_quat_uncertainty_.q1*curr_filter_quat_uncertainty_.q1; - //ekf_imu_msg_.orientation_covariance[7] = curr_filter_quat_uncertainty_.q3*curr_filter_quat_uncertainty_.q3; - - } break; - - - // Filter Status - case MIP_FILTER_DATA_FILTER_STATUS: - { - memcpy(&curr_filter_status_, field_data, sizeof(mip_filter_status)); - - //For little-endian targets, byteswap the data field - mip_filter_status_byteswap(&curr_filter_status_); - status_msg_.data.clear(); - //ROS_DEBUG_THROTTLE(1.0,"Filter Status: %#06X, Dyn. Mode: %#06X, Filter State: %#06X", - // curr_filter_status_.filter_state, - // curr_filter_status_.dynamics_mode, - // curr_filter_status_.status_flags); - status_msg_.data.push_back(curr_filter_status_.filter_state); - status_msg_.data.push_back(curr_filter_status_.dynamics_mode); - status_msg_.data.push_back(curr_filter_status_.status_flags); - status_pub_.publish(status_msg_); - - } break; - - default: break; - } - } - // Publish - ekf_pub_.publish(ekf_imu_msg_); - if(publish_rpy_) - ekf_rpy_pub_.publish(ekf_rpy_msg_); - - } break; - - - /// - //Handle checksum error packets - /// - - case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: - { - filter_checksum_error_packet_count_++; - } break; - - /// - //Handle timeout packets - /// - - case MIP_INTERFACE_CALLBACK_TIMEOUT: - { - filter_timeout_packet_count_++; - } break; - default: break; - } - - print_packet_stats(); + /// + //Handle valid packets + /// + + case MIP_INTERFACE_CALLBACK_VALID_PACKET: + { + filter_valid_packet_count_++; + + /// + //Loop through all of the data fields + /// + + while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + { + + + /// + // Decode the field + /// + switch(field_header->descriptor) + { + ////////////////////////////////////////////////////////////// + // Accelerations + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_COMPENSATED_ACCELERATION: + { + memcpy(&curr_filter_accel_, + field_data, + sizeof(mip_filter_compensated_acceleration)); + + //For little-endian targets, byteswap the data field + mip_filter_compensated_acceleration_byteswap(&curr_filter_accel_); + + // Stuff into ROS message - acceleration in m/s^2 + // Header + ekf_imu_msg_.header.seq = filter_valid_packet_count_; + ekf_imu_msg_.header.stamp = ros::Time::now(); + ekf_imu_msg_.header.frame_id = imu_frame_id_; + if (tf_ned_to_enu_) + { + ekf_imu_msg_.linear_acceleration.x = curr_filter_accel_.y; + ekf_imu_msg_.linear_acceleration.y = curr_filter_accel_.x; + ekf_imu_msg_.linear_acceleration.z = -curr_filter_accel_.z; + } + else + { + ekf_imu_msg_.linear_acceleration.x = curr_filter_accel_.x; + ekf_imu_msg_.linear_acceleration.y = curr_filter_accel_.y; + ekf_imu_msg_.linear_acceleration.z = curr_filter_accel_.z; + } + + //Sensor does not produce this value + ekf_imu_msg_.linear_acceleration_covariance = linear_accel_covariance; + ekf_rpy_msg_.header.seq = filter_valid_packet_count_; + ekf_rpy_msg_.header.stamp = ros::Time::now(); + ekf_rpy_msg_.header.frame_id = imu_frame_id_; + } break; + + ////////////////////////////////////////////////////////////// + // Estimated Attitude, Euler Angles + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_ATT_EULER_ANGLES: + { + memcpy(&curr_filter_angles_, + field_data, + sizeof(mip_filter_attitude_euler_angles)); + + //For little-endian targets, byteswap the data field + mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_); + + if (tf_ned_to_enu_) + { + ekf_rpy_msg_.vector.x = curr_filter_angles_.pitch;//Roll in ENU = Pitch in NED + ekf_rpy_msg_.vector.y = curr_filter_angles_.roll; //Pitch in ENU = Roll in NED + ekf_rpy_msg_.vector.z = -curr_filter_angles_.yaw; //Yaw in ENU = -Yaw in NED + } + else + { + ekf_rpy_msg_.vector.x = curr_filter_angles_.roll; + ekf_rpy_msg_.vector.y = curr_filter_angles_.pitch; + ekf_rpy_msg_.vector.z = curr_filter_angles_.yaw; + } + } break; + + ////////////////////////////////////////////////////////////// + // Quaternion + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_ATT_QUATERNION: + { + memcpy(&curr_filter_quaternion_, + field_data, + sizeof(mip_filter_attitude_quaternion)); + + //For little-endian targets, byteswap the data field + mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); + + if (tf_ned_to_enu_) + { + ekf_imu_msg_.orientation.x = curr_filter_quaternion_.q[2]; + ekf_imu_msg_.orientation.y = curr_filter_quaternion_.q[1]; + ekf_imu_msg_.orientation.z = -1.0*curr_filter_quaternion_.q[3]; + ekf_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; + } + else + { + ekf_imu_msg_.orientation.x = curr_filter_quaternion_.q[1]; + ekf_imu_msg_.orientation.y = curr_filter_quaternion_.q[2]; + ekf_imu_msg_.orientation.z = curr_filter_quaternion_.q[3]; + ekf_imu_msg_.orientation.w = curr_filter_quaternion_.q[0]; + } + + } break; + + ////////////////////////////////////////////////////////////// + // Angular Rates + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE: + { + memcpy(&curr_filter_angular_rate_, + field_data, + sizeof(mip_filter_compensated_angular_rate)); + + //For little-endian targets, byteswap the data field + mip_filter_compensated_angular_rate_byteswap(&curr_filter_angular_rate_); + + if (tf_ned_to_enu_) + { + ekf_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.y; + ekf_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.x; + ekf_imu_msg_.angular_velocity.z = -curr_filter_angular_rate_.z; + } + else + { + ekf_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.x; + ekf_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.y; + ekf_imu_msg_.angular_velocity.z = curr_filter_angular_rate_.z; + } + //Sensor does not produce this value + ekf_imu_msg_.angular_velocity_covariance = angular_vel_covariance; + + } break; + + ////////////////////////////////////////////////////////////// + // Attitude Uncertainty (Euler) + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER: + { + memcpy(&curr_filter_att_uncertainty_, + field_data, + sizeof(mip_filter_euler_attitude_uncertainty)); + + //For little-endian targets, byteswap the data field + mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_); + if (tf_ned_to_enu_) + { + ekf_imu_msg_.orientation_covariance[0] = + curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; + ekf_imu_msg_.orientation_covariance[4] = + curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; + ekf_imu_msg_.orientation_covariance[8] = + curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; + } + else + { + ekf_imu_msg_.orientation_covariance[0] = + curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; + ekf_imu_msg_.orientation_covariance[4] = + curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; + ekf_imu_msg_.orientation_covariance[8] = + curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; + } + + } break; + + + ////////////////////////////////////////////////////////////// + // Attitude Uncertainty (Quaternion) + ////////////////////////////////////////////////////////////// + case MIP_FILTER_DATA_ATT_UNCERTAINTY_QUATERNION: + { + memcpy(&curr_filter_quat_uncertainty_, + field_data, + sizeof(mip_filter_quaternion_attitude_uncertainty)); + + //For little-endian targets, byteswap the data field + mip_filter_quaternion_attitude_uncertainty_byteswap(&curr_filter_quat_uncertainty_); + + //ekf_imu_msg_.orientation_covariance[1] = curr_filter_quat_uncertainty_.q2*curr_filter_quat_uncertainty_.q2; + //ekf_imu_msg_.orientation_covariance[5] = curr_filter_quat_uncertainty_.q1*curr_filter_quat_uncertainty_.q1; + //ekf_imu_msg_.orientation_covariance[7] = curr_filter_quat_uncertainty_.q3*curr_filter_quat_uncertainty_.q3; + + } break; + + + // Filter Status + case MIP_FILTER_DATA_FILTER_STATUS: + { + memcpy(&curr_filter_status_, + field_data, + sizeof(mip_filter_status)); + + //For little-endian targets, byteswap the data field + mip_filter_status_byteswap(&curr_filter_status_); + status_msg_.data.clear(); + //ROS_DEBUG_THROTTLE(1.0,"Filter Status: %#06X, Dyn. Mode: %#06X, Filter State: %#06X", + // curr_filter_status_.filter_state, + // curr_filter_status_.dynamics_mode, + // curr_filter_status_.status_flags); + status_msg_.data.push_back(curr_filter_status_.filter_state); + status_msg_.data.push_back(curr_filter_status_.dynamics_mode); + status_msg_.data.push_back(curr_filter_status_.status_flags); + status_pub_.publish(status_msg_); + + if(curr_filter_status_.filter_state == 2 && initilized) + { + solution_valid = true; + } + else if(initilized)// We are initized but unstable + { + ROS_WARN_THROTTLE(1.0, "IMU Filter unstable... Filter Status: %#06X, Dyn. Mode: %#06X, Filter State: %#06X", + curr_filter_status_.filter_state, + curr_filter_status_.dynamics_mode, + curr_filter_status_.status_flags); + } + + } break; + + default: break; + } + } + + if(solution_valid)//Internal sensor EKF solution + { + // Publish + ekf_pub_.publish(ekf_imu_msg_); + if(publish_rpy_) + { + ekf_rpy_pub_.publish(ekf_rpy_msg_); + } + } + + } break; + + + /// + //Handle checksum error packets + /// + + case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: + { + filter_checksum_error_packet_count_++; + } break; + + /// + //Handle timeout packets + /// + + case MIP_INTERFACE_CALLBACK_TIMEOUT: + { + filter_timeout_packet_count_++; + } break; + + default: break; + } + + print_packet_stats(); } // filter_packet_callback @@ -816,195 +1007,238 @@ void Microstrain::ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_si u16 field_offset = 0; // If we aren't publishing, then return if (!publish_cf_ && !publish_mag_) + { return; + } + //The packet callback can have several types, process them all switch(callback_type) { - /// - //Handle valid packets - /// - - case MIP_INTERFACE_CALLBACK_VALID_PACKET: - { - ahrs_valid_packet_count_++; - - /// - //Loop through all of the data fields - /// - - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) - { - - /// - // Decode the field - /// - - switch(field_header->descriptor) - { - /// - // Scaled Accelerometer - /// - - case MIP_AHRS_DATA_ACCEL_SCALED: - { - memcpy(&curr_ahrs_accel_, field_data, sizeof(mip_ahrs_scaled_accel)); - - //For little-endian targets, byteswap the data field - mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel_); - - // Stuff into ROS message - acceleration in m/s^2 - // Header - cf_imu_msg_.header.seq = ahrs_valid_packet_count_; - cf_imu_msg_.header.stamp = ros::Time::now(); - cf_imu_msg_.header.frame_id = imu_frame_id_; - if (tf_ned_to_enu_) - { - cf_imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[1]; //SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[0]; - cf_imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[0]; //SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[1]; - cf_imu_msg_.linear_acceleration.z = -9.81*curr_ahrs_accel_.scaled_accel[2]; //SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[2]; - } - else - { - cf_imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0]; //SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[0]; - cf_imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1]; //SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[1]; - cf_imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2]; - } - mag_msg_.header.seq = ahrs_valid_packet_count_; //SJP Added - mag_msg_.header.stamp = ros::Time::now(); //SJP Added - mag_msg_.header.frame_id = imu_frame_id_; - cf_rpy_msg_.header.seq = ahrs_valid_packet_count_; //SJP Added - cf_rpy_msg_.header.stamp = ros::Time::now(); //SJP Added - cf_rpy_msg_.header.frame_id = imu_frame_id_; //SJP Added - - } break; - - /// - // Scaled Gyro - /// - - case MIP_AHRS_DATA_GYRO_SCALED: - { - memcpy(&curr_ahrs_gyro_, field_data, sizeof(mip_ahrs_scaled_gyro)); - - //For little-endian targets, byteswap the data field - mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro_); - if (tf_ned_to_enu_) - { - cf_imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[1]; //SJP Modified from curr_ahrs_gyro_.scaled_gyro[0]; - cf_imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[0]; //SJP Modified from curr_ahrs_gyro_.scaled_gyro[1]; - cf_imu_msg_.angular_velocity.z = -curr_ahrs_gyro_.scaled_gyro[2]; //SJP Modified from curr_ahrs_gyro_.scaled_gyro[2]; - } - else - { - cf_imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0]; //SJP Modified from curr_ahrs_gyro_.scaled_gyro[0]; - cf_imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[1]; //SJP Modified from curr_ahrs_gyro_.scaled_gyro[1]; - cf_imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2]; - } - } break; - - /// - // Scaled Magnetometer - /// - - case MIP_AHRS_DATA_MAG_SCALED: - { - memcpy(&curr_ahrs_mag_, field_data, sizeof(mip_ahrs_scaled_mag)); - - //For little-endian targets, byteswap the data field - mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag_); - if (tf_ned_to_enu_) - { - mag_msg_.magnetic_field.x = curr_ahrs_mag_.scaled_mag[1]; //SJP Added (ENU x = NED y) - mag_msg_.magnetic_field.y = curr_ahrs_mag_.scaled_mag[0]; //SJP Added (ENU y = NED x) - mag_msg_.magnetic_field.z = -curr_ahrs_mag_.scaled_mag[2]; //SJP Added (ENU z = -NED z) - } - else - { - mag_msg_.magnetic_field.x = curr_ahrs_mag_.scaled_mag[0]; //SJP Added (ENU x = NED y) - mag_msg_.magnetic_field.y = curr_ahrs_mag_.scaled_mag[1]; //SJP Added (ENU y = NED x) - mag_msg_.magnetic_field.z = curr_ahrs_mag_.scaled_mag[2]; - } - } break; - - // Quaternion - case MIP_AHRS_DATA_QUATERNION: - { - memcpy(&curr_ahrs_quaternion_, field_data, sizeof(mip_ahrs_quaternion)); - - //For little-endian targets, byteswap the data field - mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_); - if (tf_ned_to_enu_) - { - cf_imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; - cf_imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; - cf_imu_msg_.orientation.z = -curr_ahrs_quaternion_.q[3]; - cf_imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; - } - else - { - cf_imu_msg_.orientation.x = curr_ahrs_quaternion_.q[1]; - cf_imu_msg_.orientation.y = curr_ahrs_quaternion_.q[2]; - cf_imu_msg_.orientation.z = curr_ahrs_quaternion_.q[3]; - cf_imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; - } - } break; - - - case MIP_AHRS_DATA_EULER_ANGLES: - { - memcpy(&curr_ahrs_euler_angles_, field_data, sizeof(mip_ahrs_euler_angles)); - - //For little-endian targets, byteswap the data field - mip_ahrs_euler_angles_byteswap(&curr_ahrs_euler_angles_); - if (tf_ned_to_enu_) - { - cf_rpy_msg_.vector.x = curr_ahrs_euler_angles_.pitch; //Roll in ENU = Pitch in NED - cf_rpy_msg_.vector.y = curr_ahrs_euler_angles_.roll; //Pitch in ENU = Roll in NED - cf_rpy_msg_.vector.z = -curr_ahrs_euler_angles_.yaw; //Yaw in ENU = -Yaw in NED - } - else - { - cf_rpy_msg_.vector.x = curr_ahrs_euler_angles_.roll; //Roll in ENU = Pitch in NED - cf_rpy_msg_.vector.y = curr_ahrs_euler_angles_.pitch; //Pitch in ENU = Roll in NED - cf_rpy_msg_.vector.z = curr_ahrs_euler_angles_.yaw; - } - } break; - - default: break; - } - } - - // Publish - if(publish_cf_) - { - cf_pub_.publish(cf_imu_msg_); - if(publish_rpy_) - cf_rpy_pub_.publish(cf_rpy_msg_); - } - if(publish_mag_) - mag_pub_.publish(mag_msg_); - - - } break; - - /// - //Handle checksum error packets - /// - - case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: - { - ahrs_checksum_error_packet_count_++; - } break; - - /// - //Handle timeout packets - /// - - case MIP_INTERFACE_CALLBACK_TIMEOUT: - { - ahrs_timeout_packet_count_++; - } break; - default: break; + /// + //Handle valid packets + /// + + case MIP_INTERFACE_CALLBACK_VALID_PACKET: + { + ahrs_valid_packet_count_++; + + /// + //Loop through all of the data fields + /// + + while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + { + + /// + // Decode the field + /// + + switch(field_header->descriptor) + { + /// + // Scaled Accelerometer + /// + + case MIP_AHRS_DATA_ACCEL_SCALED: + { + memcpy(&curr_ahrs_accel_, + field_data, + sizeof(mip_ahrs_scaled_accel)); + + //For little-endian targets, byteswap the data field + mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel_); + + // Stuff into ROS message - acceleration in m/s^2 + // Header + cf_imu_msg_.header.seq = ahrs_valid_packet_count_; + cf_imu_msg_.header.stamp = ros::Time::now(); + cf_imu_msg_.header.frame_id = imu_frame_id_; + if (tf_ned_to_enu_) + { + cf_imu_msg_.linear_acceleration.x = + 9.81*curr_ahrs_accel_.scaled_accel[1];//SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[0]; + cf_imu_msg_.linear_acceleration.y = + 9.81*curr_ahrs_accel_.scaled_accel[0];//SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[1]; + cf_imu_msg_.linear_acceleration.z = + -9.81*curr_ahrs_accel_.scaled_accel[2];//SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[2]; + } + else + { + cf_imu_msg_.linear_acceleration.x = + 9.81*curr_ahrs_accel_.scaled_accel[0];//SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[0]; + cf_imu_msg_.linear_acceleration.y = + 9.81*curr_ahrs_accel_.scaled_accel[1];//SJP Modified from 9.81*curr_ahrs_accel_.scaled_accel[1]; + cf_imu_msg_.linear_acceleration.z = + 9.81*curr_ahrs_accel_.scaled_accel[2]; + } + + //Sensor does not produce this value + cf_imu_msg_.linear_acceleration_covariance = linear_accel_covariance; + + mag_msg_.header.seq = ahrs_valid_packet_count_;//SJP Added + mag_msg_.header.stamp = ros::Time::now();//SJP Added + mag_msg_.header.frame_id = imu_frame_id_; + cf_rpy_msg_.header.seq = ahrs_valid_packet_count_;//SJP Added + cf_rpy_msg_.header.stamp = ros::Time::now();//SJP Added + cf_rpy_msg_.header.frame_id = imu_frame_id_;//SJP Added + + } break; + + /// + // Scaled Gyro + /// + + case MIP_AHRS_DATA_GYRO_SCALED: + { + memcpy(&curr_ahrs_gyro_, + field_data, + sizeof(mip_ahrs_scaled_gyro)); + + //For little-endian targets, byteswap the data field + mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro_); + if (tf_ned_to_enu_) + { + cf_imu_msg_.angular_velocity.x = + curr_ahrs_gyro_.scaled_gyro[1];//SJP Modified from curr_ahrs_gyro_.scaled_gyro[0]; + cf_imu_msg_.angular_velocity.y = + curr_ahrs_gyro_.scaled_gyro[0];//SJP Modified from curr_ahrs_gyro_.scaled_gyro[1]; + cf_imu_msg_.angular_velocity.z = + -curr_ahrs_gyro_.scaled_gyro[2];//SJP Modified from curr_ahrs_gyro_.scaled_gyro[2]; + } + else + { + cf_imu_msg_.angular_velocity.x = + curr_ahrs_gyro_.scaled_gyro[0];//SJP Modified from curr_ahrs_gyro_.scaled_gyro[0]; + cf_imu_msg_.angular_velocity.y = + curr_ahrs_gyro_.scaled_gyro[1];//SJP Modified from curr_ahrs_gyro_.scaled_gyro[1]; + cf_imu_msg_.angular_velocity.z = + curr_ahrs_gyro_.scaled_gyro[2]; + } + //Sensor does not produce this value + cf_imu_msg_.angular_velocity_covariance = angular_vel_covariance; + } break; + + /// + // Scaled Magnetometer + /// + + case MIP_AHRS_DATA_MAG_SCALED: + { + memcpy(&curr_ahrs_mag_, + field_data, + sizeof(mip_ahrs_scaled_mag)); + + //For little-endian targets, byteswap the data field + mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag_); + if (tf_ned_to_enu_) + { + mag_msg_.magnetic_field.x = + curr_ahrs_mag_.scaled_mag[1];//SJP Added (ENU x = NED y) + mag_msg_.magnetic_field.y = + curr_ahrs_mag_.scaled_mag[0];//SJP Added (ENU y = NED x) + mag_msg_.magnetic_field.z = + -curr_ahrs_mag_.scaled_mag[2];//SJP Added (ENU z = -NED z) + } + else + { + mag_msg_.magnetic_field.x = + curr_ahrs_mag_.scaled_mag[0];//SJP Added (ENU x = NED y) + mag_msg_.magnetic_field.y = + curr_ahrs_mag_.scaled_mag[1];//SJP Added (ENU y = NED x) + mag_msg_.magnetic_field.z = + curr_ahrs_mag_.scaled_mag[2]; + } + } break; + + // Quaternion + case MIP_AHRS_DATA_QUATERNION: + { + memcpy(&curr_ahrs_quaternion_, + field_data, + sizeof(mip_ahrs_quaternion)); + + //For little-endian targets, byteswap the data field + mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_); + if (tf_ned_to_enu_) + { + cf_imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; + cf_imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; + cf_imu_msg_.orientation.z = -curr_ahrs_quaternion_.q[3]; + cf_imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; + } + else + { + cf_imu_msg_.orientation.x = curr_ahrs_quaternion_.q[1]; + cf_imu_msg_.orientation.y = curr_ahrs_quaternion_.q[2]; + cf_imu_msg_.orientation.z = curr_ahrs_quaternion_.q[3]; + cf_imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; + } + cf_imu_msg_.orientation_covariance = cf_orientation_covariance; + } break; + + + case MIP_AHRS_DATA_EULER_ANGLES: + { + memcpy(&curr_ahrs_euler_angles_, + field_data, + sizeof(mip_ahrs_euler_angles)); + + //For little-endian targets, byteswap the data field + mip_ahrs_euler_angles_byteswap(&curr_ahrs_euler_angles_); + if (tf_ned_to_enu_) + { + cf_rpy_msg_.vector.x = curr_ahrs_euler_angles_.pitch;//Roll in ENU = Pitch in NED + cf_rpy_msg_.vector.y = curr_ahrs_euler_angles_.roll;//Pitch in ENU = Roll in NED + cf_rpy_msg_.vector.z = -curr_ahrs_euler_angles_.yaw;//Yaw in ENU = -Yaw in NED + } + else + { + cf_rpy_msg_.vector.x = curr_ahrs_euler_angles_.roll;//Roll in ENU = Pitch in NED + cf_rpy_msg_.vector.y = curr_ahrs_euler_angles_.pitch;//Pitch in ENU = Roll in NED + cf_rpy_msg_.vector.z = curr_ahrs_euler_angles_.yaw; + } + } break; + + default: break; + } + } + + // Publish + if(publish_cf_) + { + cf_pub_.publish(cf_imu_msg_); + if(publish_rpy_) + { + cf_rpy_pub_.publish(cf_rpy_msg_); + } + } + if(publish_mag_) + { + mag_pub_.publish(mag_msg_); + } + + + } break; + + /// + //Handle checksum error packets + /// + + case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: + { + ahrs_checksum_error_packet_count_++; + } break; + + /// + //Handle timeout packets + /// + + case MIP_INTERFACE_CALLBACK_TIMEOUT: + { + ahrs_timeout_packet_count_++; + } break; + + default: break; } print_packet_stats(); @@ -1013,8 +1247,11 @@ void Microstrain::ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_si void Microstrain::print_packet_stats() { - ROS_DEBUG_THROTTLE(1.0,"%u FILTER (%u errors) %u AHRS (%u errors)", filter_valid_packet_count_, filter_timeout_packet_count_ + filter_checksum_error_packet_count_, - ahrs_valid_packet_count_, ahrs_timeout_packet_count_ + ahrs_checksum_error_packet_count_); + ROS_DEBUG_THROTTLE(1.0,"%u FILTER (%u errors) %u AHRS (%u errors)", + filter_valid_packet_count_, + filter_timeout_packet_count_ + filter_checksum_error_packet_count_, + ahrs_valid_packet_count_, + ahrs_timeout_packet_count_ + ahrs_checksum_error_packet_count_); } // print_packet_stats