From 96711c18aa0e79a9be94aa6a518110a3df5c9728 Mon Sep 17 00:00:00 2001 From: Sam Date: Tue, 18 Jun 2019 15:17:33 -0400 Subject: [PATCH 1/4] Added static IMU message covariance population via parameters. --- include/microstrain_mips/microstrain_3dm.h | 3 +++ launch/microstrain.launch | 5 ++++ src/microstrain_3dm.cpp | 27 ++++++++++++++++++++-- 3 files changed, 33 insertions(+), 2 deletions(-) diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index 19b63b7d..3a59b413 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -309,6 +309,9 @@ namespace Microstrain bool publish_imu_; bool publish_odom_; bool publish_bias_; + std::vector imu_linear_cov_; + std::vector imu_angular_cov_; + std::vector imu_orientation_cov_; //Device Flags bool GX5_15; diff --git a/launch/microstrain.launch b/launch/microstrain.launch index ec87ddb2..a47c35f3 100644 --- a/launch/microstrain.launch +++ b/launch/microstrain.launch @@ -37,6 +37,11 @@ + + + [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] + [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] + [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 6396f668..7f511888 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -51,7 +51,10 @@ namespace Microstrain odom_child_frame_id_("odom_frame"), publish_gps_(true), publish_imu_(true), - publish_odom_(true) + publish_odom_(true), + imu_linear_cov_(std::vector(9,0.0)), + imu_angular_cov_(std::vector(9,0.0)), + imu_orientation_cov_(std::vector(9,0.0)) { // pass } @@ -146,9 +149,21 @@ namespace Microstrain std::string("base_link")); private_nh.param("publish_imu",publish_imu_, true); - private_nh.param("publish_bias",publish_bias_, true); + // Covariance parameters to set the sensor_msg/IMU covariance values + std::vector default_cov(9,0.0); + private_nh.param("imu_orientation_cov", imu_orientation_cov_, default_cov); + private_nh.param("imu_linear_cov",imu_linear_cov_, default_cov); + private_nh.param("imu_angular_cov",imu_angular_cov_, default_cov); + + + ROS_INFO("Publishing the values: %d, %d",default_cov.size(),imu_orientation_cov_.size()); + for( unsigned int a = 0; a < imu_orientation_cov_.size(); a = a + 1 ) + { + ROS_INFO("Imu orientation values: %f",imu_orientation_cov_[a]); + } + // ROS publishers and subscribers if (publish_imu_) imu_pub_ = node.advertise("imu/data",100); @@ -2861,6 +2876,9 @@ void Microstrain::device_status_callback() imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0]; imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1]; imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2]; + // Since the sensor does not produce a covariance for linear acceleration, set it based + // on our pulled in parameters. + std::copy( imu_linear_cov_.begin(), imu_linear_cov_.end(), imu_msg_.linear_acceleration_covariance.begin()); }break; @@ -2878,6 +2896,9 @@ void Microstrain::device_status_callback() imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0]; imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[1]; imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2]; + // Since the sensor does not produce a covariance for angular velocity, set it based + // on our pulled in parameters. + std::copy( imu_angular_cov_.begin(), imu_angular_cov_.end(), imu_msg_.angular_velocity_covariance.begin()); }break; @@ -2906,6 +2927,8 @@ void Microstrain::device_status_callback() imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; imu_msg_.orientation.z = -1.0*curr_ahrs_quaternion_.q[3]; imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; + // Since the MIP_AHRS data does not contain uncertainty values we have to set them based on the parameter values. + std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(), imu_msg_.orientation_covariance.begin()); }break; From a67bc16c96dcff021251432ca8021f4820006363 Mon Sep 17 00:00:00 2001 From: Sam Date: Tue, 18 Jun 2019 16:57:53 -0400 Subject: [PATCH 2/4] Cleanup that was forgotten in last commit. --- src/microstrain_3dm.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 7f511888..8bb75a4e 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -157,13 +157,6 @@ namespace Microstrain private_nh.param("imu_linear_cov",imu_linear_cov_, default_cov); private_nh.param("imu_angular_cov",imu_angular_cov_, default_cov); - - ROS_INFO("Publishing the values: %d, %d",default_cov.size(),imu_orientation_cov_.size()); - for( unsigned int a = 0; a < imu_orientation_cov_.size(); a = a + 1 ) - { - ROS_INFO("Imu orientation values: %f",imu_orientation_cov_[a]); - } - // ROS publishers and subscribers if (publish_imu_) imu_pub_ = node.advertise("imu/data",100); From 8702a1d0a903d26bbe3bc0043b5b3b37c998fb16 Mon Sep 17 00:00:00 2001 From: Sam Date: Tue, 18 Jun 2019 18:46:10 -0400 Subject: [PATCH 3/4] Cleaned up indentation levels, removed tabs and replaced with spaces, and updated curly brace locations according to: http://wiki.ros.org/CppStyleGuide section 6. --- include/microstrain_mips/microstrain_3dm.h | 23 +- src/microstrain_3dm.cpp | 4397 +++++++++++--------- 2 files changed, 2352 insertions(+), 2068 deletions(-) diff --git a/include/microstrain_mips/microstrain_3dm.h b/include/microstrain_mips/microstrain_3dm.h index 3a59b413..94333c04 100644 --- a/include/microstrain_mips/microstrain_3dm.h +++ b/include/microstrain_mips/microstrain_3dm.h @@ -27,14 +27,15 @@ along with Foobar. If not, see . #define _MICROSTRAIN_3DM_H // Tell compiler that the following MIP SDI are C functions -extern "C" { -#include "mip_sdk.h" -#include "byteswap_utilities.h" -#include "mip_gx4_imu.h" -#include "mip_gx4_45.h" -#include "mip_gx4_25.h" -#include "mip_sdk_3dm.h" -#include "GX4-45_Test.h" +extern "C" +{ + #include "mip_sdk.h" + #include "byteswap_utilities.h" + #include "mip_gx4_imu.h" + #include "mip_gx4_45.h" + #include "mip_gx4_25.h" + #include "mip_sdk_3dm.h" + #include "GX4-45_Test.h" } #include @@ -82,8 +83,8 @@ extern "C" { #include "microstrain_mips/SetMagDipAdaptiveVals.h" -#define MIP_SDK_GX4_45_IMU_STANDARD_MODE 0x01 -#define MIP_SDK_GX4_45_IMU_DIRECT_MODE 0x02 +#define MIP_SDK_GX4_45_IMU_STANDARD_MODE 0x01 +#define MIP_SDK_GX4_45_IMU_DIRECT_MODE 0x02 #define NUM_COMMAND_LINE_ARGUMENTS 3 @@ -241,7 +242,7 @@ namespace Microstrain private: //! @brief Reset KF service callback bool reset_callback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &resp); + std_srvs::Empty::Response &resp); //! @brief Convience for printing packet stats void print_packet_stats(); diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 8bb75a4e..37c7226e 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -20,7 +20,6 @@ along with Foobar. If not, see . */ - #include #include #include @@ -29,9 +28,6 @@ along with Foobar. If not, see . #include "microstrain_mips/status_msg.h" #include "microstrain_diagnostic_updater.h" - - - namespace Microstrain { Microstrain::Microstrain(): @@ -128,13 +124,17 @@ namespace Microstrain private_nh.param("imu_rate",imu_rate_, 10); private_nh.param("nav_rate",nav_rate_, 10); private_nh.param("dynamics_mode",pdyn_mode,1); + dynamics_mode = (u8)pdyn_mode; - if (dynamics_mode < 1 || dynamics_mode > 3){ + 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; } + 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; } @@ -146,7 +146,7 @@ namespace Microstrain private_nh.param("imu_frame_id",imu_frame_id_, std::string("base_link")); private_nh.param("odom_frame_id",odom_frame_id_, std::string("wgs84")); private_nh.param("odom_child_frame_id",odom_child_frame_id_, - std::string("base_link")); + std::string("base_link")); private_nh.param("publish_imu",publish_imu_, true); private_nh.param("publish_bias",publish_bias_, true); @@ -161,11 +161,9 @@ namespace Microstrain if (publish_imu_) imu_pub_ = node.advertise("imu/data",100); - //Publishes device status device_status_pub_ = node.advertise("device/status", 100); - //Services to set/get device functions ros::ServiceServer reset_filter = node.advertiseService("reset_kf", &Microstrain::reset_callback, this); ros::ServiceServer device_report_service = node.advertiseService("device_report", &Microstrain::device_report, this); @@ -218,13 +216,13 @@ namespace Microstrain //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){ + 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?"); } - //////////////////////////////////////// // Device setup float dT=0.5; // common sleep time after setup communications @@ -235,33 +233,41 @@ namespace Microstrain device_descriptors_size = 128*2; com_mode = MIP_SDK_GX4_45_IMU_STANDARD_MODE; start = clock(); - while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { ROS_INFO("mip_system_com_mode function timed out."); break; } } + //Verify device mode setting ROS_INFO("Verify comm's mode"); start = clock(); - while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_system_com_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { ROS_INFO("mip_system_com_mode function timed out."); break; } } + ROS_INFO("Sleep for a second..."); ros::Duration(dT).sleep(); ROS_INFO("Right mode?"); if(com_mode != MIP_SDK_GX4_45_IMU_STANDARD_MODE) { - ROS_ERROR("Appears we didn't get into standard mode!"); + ROS_ERROR("Appears we didn't get into standard mode!"); } //Get device info start = clock(); - while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { ROS_INFO("mip_base_cmd_get_device_info function timed out."); break; } @@ -273,69 +279,78 @@ namespace Microstrain ROS_INFO("Model Name => %s\n", temp_string); std::string model_name; - for(int i = 6; i < 20; i++){ + for(int i = 6; i < 20; i++) + { model_name += temp_string[i]; } //Set device model flag model_name = model_name.c_str(); - if(model_name == GX5_45_DEVICE){ + if(model_name == GX5_45_DEVICE) + { GX5_45 = true; } - if(model_name == GX5_35_DEVICE){ + if(model_name == GX5_35_DEVICE) + { GX5_35 = true; } - if(model_name == GX5_25_DEVICE){ + if(model_name == GX5_25_DEVICE) + { GX5_25 = true; } - if(model_name == GX5_15_DEVICE){ + if(model_name == GX5_15_DEVICE) + { GX5_15 = true; } - //Set GPS publishing to true if IMU model has GPS - if(GX5_45 || GX5_35){ - private_nh.param("publish_gps",publish_gps_, true); - private_nh.param("publish_odom",publish_odom_, true); - } - else{ - private_nh.param("publish_gps",publish_gps_, false); - private_nh.param("publish_odom",publish_odom_, false); - } + //Set GPS publishing to true if IMU model has GPS + if(GX5_45 || GX5_35) + { + private_nh.param("publish_gps",publish_gps_, true); + private_nh.param("publish_odom",publish_odom_, true); + } + else + { + private_nh.param("publish_gps",publish_gps_, false); + private_nh.param("publish_odom",publish_odom_, false); + } - if (publish_gps_) - gps_pub_ = node.advertise("gps/fix",100); + if(publish_gps_) + gps_pub_ = node.advertise("gps/fix",100); - if (publish_odom_) - { - nav_pub_ = node.advertise("nav/odom",100); - nav_status_pub_ = node.advertise("nav/status",100); - } + if(publish_odom_) + { + nav_pub_ = node.advertise("nav/odom",100); + nav_status_pub_ = node.advertise("nav/status",100); + } - // 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_FATAL("Can't setup filter callback!"); - return; - } - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET, this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) - { - ROS_FATAL("Can't setup ahrs callbacks!"); - return; - } + // 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_FATAL("Can't setup filter callback!"); + return; + } - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_GPS_DATA_SET, this, &gps_packet_callback_wrapper) != MIP_INTERFACE_OK) - { - ROS_FATAL("Can't setup gpscallbacks!"); - return; - } + if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET, this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) + { + ROS_FATAL("Can't setup ahrs callbacks!"); + return; + } + if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_GPS_DATA_SET, this, &gps_packet_callback_wrapper) != MIP_INTERFACE_OK) + { + ROS_FATAL("Can't setup gpscallbacks!"); + return; + } // Put into idle mode ROS_INFO("Idling Device: Stopping data streams and/or waking from sleep"); start = clock(); - while(mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { ROS_INFO("mip_base_cmd_idle function timed out."); break; } @@ -344,303 +359,369 @@ namespace Microstrain // AHRS Setup // Get base rate - if (publish_imu_){ - start = clock(); - while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_get_ahrs_base_rate function timed out."); - break; - } - } - ROS_INFO("AHRS Base Rate => %d Hz", base_rate); - ros::Duration(dT).sleep(); - // Deterimine decimation to get close to goal rate - u8 imu_decimation = (u8)((float)base_rate/ (float)imu_rate_); - ROS_INFO("AHRS decimation set to %#04X",imu_decimation); - - // AHRS Message Format - // Set 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_decimation[0] = imu_decimation;//0x32; - data_stream_format_decimation[1] = imu_decimation;//0x32; - data_stream_format_decimation[2] = imu_decimation;//0x32; - data_stream_format_num_entries = 3; - start = clock(); - 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){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Poll to verify - ROS_INFO("Poll AHRS data to verify"); - start = clock(); - 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){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_poll_ahrs function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Save - if (save_settings) - { - ROS_INFO("Saving AHRS data settings"); - start = clock(); - while(mip_3dm_cmd_ahrs_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } + if (publish_imu_) + { + start = clock(); + while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_get_ahrs_base_rate function timed out."); + break; + } + } - // Declination Source - // Set declination - ROS_INFO("Setting declination source to %#04X",declination_source_u8); - start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_declination_source function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - //Read back the declination source - ROS_INFO("Reading back declination source"); - start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_declination_source function timed out."); - break; - } - } - 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"); - start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_declination_source function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } + ROS_INFO("AHRS Base Rate => %d Hz", base_rate); + ros::Duration(dT).sleep(); + // Deterimine decimation to get close to goal rate + u8 imu_decimation = (u8)((float)base_rate/ (float)imu_rate_); + ROS_INFO("AHRS decimation set to %#04X",imu_decimation); + + // AHRS Message Format + // Set 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_decimation[0] = imu_decimation;//0x32; + data_stream_format_decimation[1] = imu_decimation;//0x32; + data_stream_format_decimation[2] = imu_decimation;//0x32; + data_stream_format_num_entries = 3; + + start = clock(); + 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) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Poll to verify + ROS_INFO("Poll AHRS data to verify"); + start = clock(); + 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) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_poll_ahrs function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Save + if (save_settings) + { + ROS_INFO("Saving AHRS data settings"); + start = clock(); + while(mip_3dm_cmd_ahrs_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } + + // Declination Source + // Set declination + ROS_INFO("Setting declination source to %#04X",declination_source_u8); + start = clock(); + while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_declination_source function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + //Read back the declination source + ROS_INFO("Reading back declination source"); + start = clock(); + while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_declination_source function timed out."); + break; + } + } + + 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"); + start = clock(); + while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_declination_source function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } } // end of AHRS setup + // GPS Setup - if (publish_gps_){ - start = clock(); - while(mip_3dm_cmd_get_gps_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_get_gps_base_rate function timed out."); - break; - } - } - ROS_INFO("GPS Base Rate => %d Hz", base_rate); - u8 gps_decimation = (u8)((float)base_rate/ (float)gps_rate_); - ros::Duration(dT).sleep(); - - ////////// GPS Message Format - // Set - ROS_INFO("Setting GPS stream format"); - data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS; - data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY; - data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME; - data_stream_format_decimation[0] = gps_decimation; //0x01; //0x04; - data_stream_format_decimation[1] = gps_decimation; //0x01; //0x04; - data_stream_format_decimation[2] = gps_decimation; //0x01; //0x04; - data_stream_format_num_entries = 3; - start = clock(); - while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gps_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Save - if (save_settings) - { - ROS_INFO("Saving GPS data settings"); - start = clock(); - while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gps_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } + if (publish_gps_) + { + start = clock(); + while(mip_3dm_cmd_get_gps_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_get_gps_base_rate function timed out."); + break; + } + } + + ROS_INFO("GPS Base Rate => %d Hz", base_rate); + u8 gps_decimation = (u8)((float)base_rate/ (float)gps_rate_); + ros::Duration(dT).sleep(); + + ////////// GPS Message Format + // Set + ROS_INFO("Setting GPS stream format"); + data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS; + data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY; + data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME; + data_stream_format_decimation[0] = gps_decimation; //0x01; //0x04; + data_stream_format_decimation[1] = gps_decimation; //0x01; //0x04; + data_stream_format_decimation[2] = gps_decimation; //0x01; //0x04; + data_stream_format_num_entries = 3; + start = clock(); + + while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_gps_message_format function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Save + if (save_settings) + { + ROS_INFO("Saving GPS data settings"); + start = clock(); + while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_gps_message_format function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } } // end of GPS setup - if (publish_odom_){ - start = clock(); - while(mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_get_filter_base_rate function timed out."); - break; - } - } - ROS_INFO("FILTER Base Rate => %d Hz", base_rate); - u8 nav_decimation = (u8)((float)base_rate/ (float)nav_rate_); - ros::Duration(dT).sleep(); - - ////////// Filter Message Format - // Set - ROS_INFO("Setting Filter stream format"); - data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS; - data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL; - //data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; - data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; - data_stream_format_descriptors[3] = MIP_FILTER_DATA_POS_UNCERTAINTY; - data_stream_format_descriptors[4] = MIP_FILTER_DATA_VEL_UNCERTAINTY; - data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; - data_stream_format_descriptors[6] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; - data_stream_format_descriptors[7] = MIP_FILTER_DATA_FILTER_STATUS; - data_stream_format_decimation[0] = nav_decimation; //0x32; - data_stream_format_decimation[1] = nav_decimation; //0x32; - data_stream_format_decimation[2] = nav_decimation; //0x32; - data_stream_format_decimation[3] = nav_decimation; //0x32; - data_stream_format_decimation[4] = nav_decimation; //0x32; - data_stream_format_decimation[5] = nav_decimation; //0x32; - data_stream_format_decimation[6] = nav_decimation; //0x32; - data_stream_format_decimation[7] = nav_decimation; //0x32; - data_stream_format_num_entries = 8; - start = clock(); - 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){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Poll to verify - ROS_INFO("Poll filter data to test stream"); - start = clock(); - 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){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_poll_filter function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Save - if (save_settings) - { - ROS_INFO("Saving Filter data settings"); - start = clock(); - while(mip_3dm_cmd_filter_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } - // Dynamics Mode - // Set dynamics mode - ROS_INFO("Setting dynamics mode to %#04X",dynamics_mode); - start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - // Readback dynamics mode - if (readback_settings) - { - // Read the settings back - ROS_INFO("Reading back dynamics mode setting"); - start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; - } - } - 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"); - start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } + // Filter setup + if (publish_odom_) + { + start = clock(); + while(mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_get_filter_base_rate function timed out."); + break; + } + } - //Set heading Source - ROS_INFO("Set heading source to internal mag."); - heading_source = 0x1; - ROS_INFO("Setting heading source to %#04X",heading_source); - start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_heading_source function timed out."); - break; - } - } - //Read back heading source - ros::Duration(dT).sleep(); - ROS_INFO("Read back heading source..."); - start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_headingsource)!= MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_heading_source function timed out."); - break; - } - } - ROS_INFO("Heading source = %#04X",readback_headingsource); - ros::Duration(dT).sleep(); + ROS_INFO("FILTER Base Rate => %d Hz", base_rate); + u8 nav_decimation = (u8)((float)base_rate/ (float)nav_rate_); + ros::Duration(dT).sleep(); + + ////////// Filter Message Format + // Set + ROS_INFO("Setting Filter stream format"); + data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS; + data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL; + //data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; + data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; + data_stream_format_descriptors[3] = MIP_FILTER_DATA_POS_UNCERTAINTY; + data_stream_format_descriptors[4] = MIP_FILTER_DATA_VEL_UNCERTAINTY; + data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; + data_stream_format_descriptors[6] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; + data_stream_format_descriptors[7] = MIP_FILTER_DATA_FILTER_STATUS; + data_stream_format_decimation[0] = nav_decimation; //0x32; + data_stream_format_decimation[1] = nav_decimation; //0x32; + data_stream_format_decimation[2] = nav_decimation; //0x32; + data_stream_format_decimation[3] = nav_decimation; //0x32; + data_stream_format_decimation[4] = nav_decimation; //0x32; + data_stream_format_decimation[5] = nav_decimation; //0x32; + data_stream_format_decimation[6] = nav_decimation; //0x32; + data_stream_format_decimation[7] = nav_decimation; //0x32; + data_stream_format_num_entries = 8; - if (save_settings) - { - ROS_INFO("Saving heading source to EEPROM"); - start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL)!= MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_heading_source function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } + start = clock(); + 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) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Poll to verify + ROS_INFO("Poll filter data to test stream"); + start = clock(); + 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) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_poll_filter function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Save + if (save_settings) + { + ROS_INFO("Saving Filter data settings"); + start = clock(); + while(mip_3dm_cmd_filter_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } + + // Dynamics Mode + // Set dynamics mode + ROS_INFO("Setting dynamics mode to %#04X",dynamics_mode); + start = clock(); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); + break; + } + } + + ros::Duration(dT).sleep(); + // Readback dynamics mode + if (readback_settings) + { + // Read the settings back + ROS_INFO("Reading back dynamics mode setting"); + start = clock(); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); + break; + } + } + + 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"); + start = clock(); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } + + //Set heading Source + ROS_INFO("Set heading source to internal mag."); + heading_source = 0x1; + ROS_INFO("Setting heading source to %#04X",heading_source); + start = clock(); + while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_heading_source function timed out."); + break; + } + } + //Read back heading source + ros::Duration(dT).sleep(); + ROS_INFO("Read back heading source..."); + start = clock(); + while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_headingsource)!= MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_heading_source function timed out."); + break; + } + } + + ROS_INFO("Heading source = %#04X",readback_headingsource); + ros::Duration(dT).sleep(); + + if (save_settings) + { + ROS_INFO("Saving heading source to EEPROM"); + start = clock(); + while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL)!= MIP_INTERFACE_OK) + { + if (clock() - start > 5000) + { + ROS_INFO("mip_filter_heading_source function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } } // end of Filter setup // Set auto-initialization based on ROS parameter ROS_INFO("Setting auto-initinitalization to: %#04X",auto_init); auto_init_u8 = auto_init; // convert bool to u8 start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &auto_init_u8) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &auto_init_u8) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_auto_initialization function timed out."); break; } @@ -649,103 +730,124 @@ namespace Microstrain if (readback_settings) { - // Read the settings back - ROS_INFO("Reading back auto-initialization value"); - start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_auto_init)!= MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_auto_initialization function timed out."); - break; - } - } - 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); + // Read the settings back + ROS_INFO("Reading back auto-initialization value"); + start = clock(); + while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_auto_init)!= MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_auto_initialization function timed out."); + break; + } + } + + 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); } + if (save_settings) { - ROS_INFO("Saving auto init. settings to EEPROM"); - start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_auto_initialization function timed out."); - break; - } - } - ros::Duration(dT).sleep(); + ROS_INFO("Saving auto init. settings to EEPROM"); + start = clock(); + while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_auto_initialization function timed out."); + break; + } + } + ros::Duration(dT).sleep(); } // Enable Data streams dT = 0.25; - if (publish_imu_){ - ROS_INFO("Enabling AHRS stream"); - enable = 0x01; - start = clock(); - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); - break; - } - } - ros::Duration(dT).sleep(); - } - if (publish_odom_){ - ROS_INFO("Enabling Filter stream"); - enable = 0x01; - start = clock(); - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); - break; - } - } - ros::Duration(dT).sleep(); + if (publish_imu_) + { + ROS_INFO("Enabling AHRS stream"); + enable = 0x01; + start = clock(); + while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); + break; + } + } + ros::Duration(dT).sleep(); } - if (publish_gps_){ - ROS_INFO("Enabling GPS stream"); - enable = 0x01; - start = clock(); - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); - break; - } - } - ros::Duration(dT).sleep(); + + if (publish_odom_) + { + ROS_INFO("Enabling Filter stream"); + enable = 0x01; + start = clock(); + while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); + break; + } + } + ros::Duration(dT).sleep(); } - ROS_INFO("End of device setup - starting streaming"); - } - else - { - ROS_INFO("Skipping device setup and listing for existing streams"); + if (publish_gps_) + { + ROS_INFO("Enabling GPS stream"); + enable = 0x01; + start = clock(); + while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); + break; + } + } + ros::Duration(dT).sleep(); + } + + ROS_INFO("End of device setup - starting streaming"); + } + else + { + ROS_INFO("Skipping device setup and listing for existing streams"); } // end of device_setup // Reset filter - should be for either the KF or CF ROS_INFO("Reset filter"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_reset_filter function timed out."); break; } } ros::Duration(dT).sleep(); - // Loop // Determine loop rate as 2*(max update rate), but abs. max of 1kHz int max_rate = 1; - if (publish_imu_){ + if (publish_imu_) + { max_rate = std::max(max_rate,imu_rate_); } - if (publish_gps_){ + if (publish_gps_) + { max_rate = std::max(max_rate,gps_rate_); } - if (publish_odom_){ + if (publish_odom_) + { max_rate = std::max(max_rate,nav_rate_); } int spin_rate = std::min(3*max_rate,1000); @@ -754,11 +856,13 @@ namespace Microstrain microstrain_mips::RosDiagnosticUpdater ros_diagnostic_updater(this); - while (ros::ok()){ + while (ros::ok()) + { //Update the parser (this function reads the port and parses the bytes mip_interface_update(&device_interface_); - if(GX5_25){ + if(GX5_25) + { device_status_callback(); } @@ -773,490 +877,550 @@ namespace Microstrain } // End of ::run() bool Microstrain::reset_callback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &resp) + std_srvs::Empty::Response &resp) { ROS_INFO("Reseting the filter"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_reset_filter function timed out."); break; } } - return true; } //Services to get/set values on devices //Set accel bias values bool Microstrain::set_accel_bias(microstrain_mips::SetAccelBias::Request &req, microstrain_mips::SetAccelBias::Response &res) - { - ROS_INFO("Setting accel bias values"); - memset(field_data, 0, 3*sizeof(float)); - start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); - break; - } - } - ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); - - field_data[0] = req.bias.x; - field_data[1] = req.bias.y; - field_data[2] = req.bias.z; + { + ROS_INFO("Setting accel bias values"); + memset(field_data, 0, 3*sizeof(float)); + start = clock(); + while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); + break; + } + } - start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); - break; - } - } - memset(field_data, 0, 3*sizeof(float)); - start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); - break; - } - } - ROS_INFO("New accel bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); - res.success = true; - return true; - } + field_data[0] = req.bias.x; + field_data[1] = req.bias.y; + field_data[2] = req.bias.z; - //Get accel bias values - bool Microstrain::get_accel_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + start = clock(); + while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) { - ROS_INFO("Getting accel bias values"); - memset(field_data, 0, 3*sizeof(float)); + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); + break; + } + } - start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); - break; - } + memset(field_data, 0, 3*sizeof(float)); + start = clock(); + while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); + break; } - ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - - res.success = true; - return true; } + ROS_INFO("New accel bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); + + res.success = true; + return true; + } + + //Get accel bias values + bool Microstrain::get_accel_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + ROS_INFO("Getting accel bias values"); + memset(field_data, 0, 3*sizeof(float)); - //Set gyro bias values - bool Microstrain::set_gyro_bias(microstrain_mips::SetGyroBias::Request &req, microstrain_mips::SetGyroBias::Response &res) + start = clock(); + while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - ROS_INFO("Setting gyro bias values"); - memset(field_data, 0, 3*sizeof(float)); + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); + break; + } + } + ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); - break; - } + res.success = true; + return true; + } + + //Set gyro bias values + bool Microstrain::set_gyro_bias(microstrain_mips::SetGyroBias::Request &req, microstrain_mips::SetGyroBias::Response &res) + { + ROS_INFO("Setting gyro bias values"); + memset(field_data, 0, 3*sizeof(float)); + + start = clock(); + while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); + break; } - ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); + } - field_data[0] = req.bias.x; - field_data[1] = req.bias.y; - field_data[2] = req.bias.z; + ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); - start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); - break; - } + field_data[0] = req.bias.x; + field_data[1] = req.bias.y; + field_data[2] = req.bias.z; + + start = clock(); + while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); + break; } - memset(field_data, 0, 3*sizeof(float)); - start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); - break; - } + } + memset(field_data, 0, 3*sizeof(float)); + start = clock(); + while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); + break; } - ROS_INFO("New gyro bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); - - res.success = true; - return true; } + ROS_INFO("New gyro bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); - //Get gyro bias values - bool Microstrain::get_gyro_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - ROS_INFO("Getting gyro bias values"); - memset(field_data, 0, 3*sizeof(float)); - - start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); - break; - } - } - ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + res.success = true; + return true; + } - res.success = true; - return true; - } + //Get gyro bias values + bool Microstrain::get_gyro_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + ROS_INFO("Getting gyro bias values"); + memset(field_data, 0, 3*sizeof(float)); - //Set hard iron values - bool Microstrain::set_hard_iron_values(microstrain_mips::SetHardIronValues::Request &req, microstrain_mips::SetHardIronValues::Response &res) + start = clock(); + while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } + ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); + break; + } + } + ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Setting hard iron values"); - float field_data[3] = {0}; + res.success = true; + return true; + } - start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); - break; - } - } - ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); - - field_data[0] = req.bias.x; - field_data[1] = req.bias.y; - field_data[2] = req.bias.z; - - start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); - break; - } - } - memset(field_data, 0, 3*sizeof(float)); - start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); - break; - } - } - ROS_INFO("New hard iron values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); + //Set hard iron values + bool Microstrain::set_hard_iron_values(microstrain_mips::SetHardIronValues::Request &req, microstrain_mips::SetHardIronValues::Response &res) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - res.success = true; - return true; - } + ROS_INFO("Setting hard iron values"); + float field_data[3] = {0}; - //Get hard iron values - bool Microstrain::get_hard_iron_values(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + start = clock(); + while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { + ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); + break; + } + } - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } - - ROS_INFO("Getting hard iron values"); - memset(field_data, 0, 3*sizeof(float)); + ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); - start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); - break; - } - } - ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + field_data[0] = req.bias.x; + field_data[1] = req.bias.y; + field_data[2] = req.bias.z; - res.success = true; - return true; + start = clock(); + while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); + break; } + } - - //Get device report - bool Microstrain::device_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + memset(field_data, 0, 3*sizeof(float)); + start = clock(); + while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - start = clock(); - while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_base_cmd_get_device_info function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); + break; } - ROS_INFO("\n\nDevice Info:\n"); - - memset(temp_string, 0, 20*sizeof(int)); - - memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2); - ROS_INFO("Model Name => %s\n", temp_string); - - memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); - ROS_INFO("Model Number => %s\n", temp_string); - - memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); - ROS_INFO("Serial Number => %s\n", temp_string); - - memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2); - ROS_INFO("Lot Number => %s\n", temp_string); + } + ROS_INFO("New hard iron values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); - memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2); - ROS_INFO("Options => %s\n", temp_string); + res.success = true; + return true; + } - ROS_INFO("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000, (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100); + //Get hard iron values + bool Microstrain::get_hard_iron_values(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { - res.success = true; + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } - //Capture gyro bias values - bool Microstrain::gyro_bias_capture(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + ROS_INFO("Getting hard iron values"); + memset(field_data, 0, 3*sizeof(float)); + + start = clock(); + while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - memset(field_data, 0, 3*sizeof(float)); - ROS_INFO("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n"); - duration = 5000; //milliseconds - start = clock(); - while(mip_3dm_cmd_capture_gyro_bias(&device_interface_, duration, field_data) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_capture_gyro_bias function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); + break; } - ROS_INFO("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", field_data[0], field_data[1], field_data[2]); - - res.success = true; - return true; } + ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + + res.success = true; + return true; + } + - //Set soft iron matrix values - bool Microstrain::set_soft_iron_matrix(microstrain_mips::SetSoftIronMatrix::Request &req, microstrain_mips::SetSoftIronMatrix::Response &res) + //Get device report + bool Microstrain::device_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + start = clock(); + while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_base_cmd_get_device_info function timed out."); + break; } + } - memset(soft_iron, 0, 9*sizeof(float)); - memset(soft_iron_readback, 0, 9*sizeof(float)); + ROS_INFO("\n\nDevice Info:\n"); - ROS_INFO("Setting the soft iron matrix values\n"); + memset(temp_string, 0, 20*sizeof(int)); - soft_iron[0] = req.soft_iron_1.x; - soft_iron[1] = req.soft_iron_1.y; - soft_iron[2] = req.soft_iron_1.z; - soft_iron[3] = req.soft_iron_2.x; - soft_iron[4] = req.soft_iron_2.y; - soft_iron[5] = req.soft_iron_2.z; - soft_iron[6] = req.soft_iron_3.x; - soft_iron[7] = req.soft_iron_3.y; - soft_iron[8] = req.soft_iron_3.z; + memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2); + ROS_INFO("Model Name => %s\n", temp_string); - start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); - break; - } - } + memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); + ROS_INFO("Model Number => %s\n", temp_string); - //Read back the soft iron matrix values - start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); - break; - } - } + memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); + ROS_INFO("Serial Number => %s\n", temp_string); - if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) && - (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) && - (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) && - (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) && - (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) && - (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) && - (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) && - (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) && - (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001)) - { - ROS_INFO("Soft iron matrix values successfully set.\n"); - ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); - ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); - } - else - { - ROS_INFO("ERROR: Failed to set hard iron values!!!\n"); - ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); - ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); - } - res.success = true; - return true; - } + memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2); + ROS_INFO("Lot Number => %s\n", temp_string); - //Get soft iron matrix values - bool Microstrain::get_soft_iron_matrix(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } + memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2); + ROS_INFO("Options => %s\n", temp_string); - memset(soft_iron, 0, 9*sizeof(float)); - memset(soft_iron_readback, 0, 9*sizeof(float)); + ROS_INFO("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000, (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100); - ROS_INFO("Getting the soft iron matrix values\n"); + res.success = true; + return true; + } - start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); - break; - } - } + //Capture gyro bias values + bool Microstrain::gyro_bias_capture(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + memset(field_data, 0, 3*sizeof(float)); + ROS_INFO("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n"); + duration = 5000; //milliseconds + start = clock(); + while(mip_3dm_cmd_capture_gyro_bias(&device_interface_, duration, field_data) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_capture_gyro_bias function timed out."); + break; + } + } + ROS_INFO("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Soft iron matrix values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); - res.success = true; - return true; - } + res.success = true; + return true; + } - //Set complementary filter values - bool Microstrain::set_complementary_filter(microstrain_mips::SetComplementaryFilter::Request &req, microstrain_mips::SetComplementaryFilter::Response &res) + //Set soft iron matrix values + bool Microstrain::set_soft_iron_matrix(microstrain_mips::SetSoftIronMatrix::Request &req, microstrain_mips::SetSoftIronMatrix::Response &res) + { + if(GX5_15 == true) { - ROS_INFO("Setting the complementary filter values\n"); + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - comp_filter_command.north_compensation_enable = req.north_comp_enable; - comp_filter_command.up_compensation_enable = req.up_comp_enable; - comp_filter_command.north_compensation_time_constant = req.north_comp_time_const; - comp_filter_command.up_compensation_time_constant = req.up_comp_time_const; + memset(soft_iron, 0, 9*sizeof(float)); + memset(soft_iron_readback, 0, 9*sizeof(float)); - start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); - break; - } - } + ROS_INFO("Setting the soft iron matrix values\n"); - //Read back the complementary filter values - start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); - break; - } - } + soft_iron[0] = req.soft_iron_1.x; + soft_iron[1] = req.soft_iron_1.y; + soft_iron[2] = req.soft_iron_1.z; + soft_iron[3] = req.soft_iron_2.x; + soft_iron[4] = req.soft_iron_2.y; + soft_iron[5] = req.soft_iron_2.z; + soft_iron[6] = req.soft_iron_3.x; + soft_iron[7] = req.soft_iron_3.y; + soft_iron[8] = req.soft_iron_3.z; - if((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) && - (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) && - (abs(comp_filter_command.north_compensation_time_constant - comp_filter_readback.north_compensation_time_constant) < 0.001) && - (abs(comp_filter_command.up_compensation_time_constant - comp_filter_readback.up_compensation_time_constant) < 0.001)) - { - ROS_INFO("Complementary filter values successfully set.\n"); - ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); - } - else - { - ROS_INFO("ERROR: Failed to set complementary filter values!!!\n"); - ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); - } - res.success = true; - return true; + start = clock(); + while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); + break; + } } - //Get complementary filter values - bool Microstrain::get_complementary_filter(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Read back the soft iron matrix values + start = clock(); + while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) { - //Read back the complementary filter values - start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); - break; - } - } - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); - res.success = true; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); + break; + } } - //Initialize filter with Euler angles - bool Microstrain::set_filter_euler(microstrain_mips::SetFilterEuler::Request &req, microstrain_mips::SetFilterEuler::Response &res) + if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) && + (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) && + (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) && + (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) && + (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) && + (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) && + (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) && + (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) && + (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001)) { - memset(angles, 0, 3*sizeof(float)); - ROS_INFO("Resetting the Filter\n"); + ROS_INFO("Soft iron matrix values successfully set.\n"); + ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); + ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], + soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + } + else + { + ROS_INFO("ERROR: Failed to set hard iron values!!!\n"); + ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); + ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], + soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + } + res.success = true; + return true; + } - start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_reset_filter function timed out."); - break; - } - } + //Get soft iron matrix values + bool Microstrain::get_soft_iron_matrix(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - ROS_INFO("Initializing the Filter with Euler angles\n"); + memset(soft_iron, 0, 9*sizeof(float)); + memset(soft_iron_readback, 0, 9*sizeof(float)); - angles[0] = req.angle.x; - angles[1] = req.angle.y; - angles[2] = req.angle.z; + ROS_INFO("Getting the soft iron matrix values\n"); - start = clock(); - while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_set_init_attitude function timed out."); - break; - } - } + start = clock(); + while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); + break; + } + } - res.success = true; - return true; + ROS_INFO("Soft iron matrix values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], + soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + res.success = true; + return true; } - //Set filter with heading angle - bool Microstrain::set_filter_heading(microstrain_mips::SetFilterHeading::Request &req, microstrain_mips::SetFilterHeading::Response &res) + //Set complementary filter values + bool Microstrain::set_complementary_filter(microstrain_mips::SetComplementaryFilter::Request &req, microstrain_mips::SetComplementaryFilter::Response &res) + { + ROS_INFO("Setting the complementary filter values\n"); + + comp_filter_command.north_compensation_enable = req.north_comp_enable; + comp_filter_command.up_compensation_enable = req.up_comp_enable; + comp_filter_command.north_compensation_time_constant = req.north_comp_time_const; + comp_filter_command.up_compensation_time_constant = req.up_comp_time_const; + + start = clock(); + while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK) { - ROS_INFO("Resetting the Filter\n"); + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); + break; + } + } - start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_reset_filter function timed out."); - break; - } - } + //Read back the complementary filter values + start = clock(); + while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); + break; + } + } + + if((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) && + (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) && + (abs(comp_filter_command.north_compensation_time_constant - comp_filter_readback.north_compensation_time_constant) < 0.001) && + (abs(comp_filter_command.up_compensation_time_constant - comp_filter_readback.up_compensation_time_constant) < 0.001)) + { + ROS_INFO("Complementary filter values successfully set.\n"); + ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + } + else + { + ROS_INFO("ERROR: Failed to set complementary filter values!!!\n"); + ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + } + res.success = true; + return true; + } - ROS_INFO("Initializing the Filter with a heading angle\n"); + //Get complementary filter values + bool Microstrain::get_complementary_filter(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + //Read back the complementary filter values + start = clock(); + while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); + break; + } + } + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + res.success = true; + return true; + } - heading_angle = req.angle; - start = clock(); - while(mip_filter_set_init_heading(&device_interface_, heading_angle) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_set_init_heading function timed out."); - break; - } - } + //Initialize filter with Euler angles + bool Microstrain::set_filter_euler(microstrain_mips::SetFilterEuler::Request &req, microstrain_mips::SetFilterEuler::Response &res) + { + memset(angles, 0, 3*sizeof(float)); + ROS_INFO("Resetting the Filter\n"); - res.success = true; - return true; + start = clock(); + while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_reset_filter function timed out."); + break; + } + } + + ROS_INFO("Initializing the Filter with Euler angles\n"); + + angles[0] = req.angle.x; + angles[1] = req.angle.y; + angles[2] = req.angle.z; + + start = clock(); + while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_set_init_attitude function timed out."); + break; + } + } + + res.success = true; + return true; + } + + //Set filter with heading angle + bool Microstrain::set_filter_heading(microstrain_mips::SetFilterHeading::Request &req, microstrain_mips::SetFilterHeading::Response &res) + { + ROS_INFO("Resetting the Filter\n"); + + start = clock(); + while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_reset_filter function timed out."); + break; + } + } + + ROS_INFO("Initializing the Filter with a heading angle\n"); + + heading_angle = req.angle; + start = clock(); + while(mip_filter_set_init_heading(&device_interface_, heading_angle) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_set_init_heading function timed out."); + break; + } } + res.success = true; + return true; + } + //Set sensor to vehicle frame transformation bool Microstrain::set_sensor_vehicle_frame_trans(microstrain_mips::SetSensorVehicleFrameTrans::Request &req, microstrain_mips::SetSensorVehicleFrameTrans::Response &res) { - if(GX5_15 == true){ + if(GX5_15 == true) + { ROS_INFO("Device does not support this feature"); res.success = false; return true; @@ -1272,8 +1436,10 @@ namespace Microstrain angles[2] = req.angle.z; start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } @@ -1281,8 +1447,10 @@ namespace Microstrain //Read back the transformation start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } @@ -1292,14 +1460,14 @@ namespace Microstrain (abs(readback_angles[1]-angles[1]) < 0.001) && (abs(readback_angles[2]-angles[2]) < 0.001)) { - ROS_INFO("Transformation successfully set.\n"); - ROS_INFO("New angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); + ROS_INFO("Transformation successfully set.\n"); + ROS_INFO("New angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); } else { - ROS_INFO("ERROR: Failed to set transformation!!!\n"); - ROS_INFO("Sent angles: %f roll %f pitch %f yaw\n", angles[0], angles[1], angles[2]); - ROS_INFO("Returned angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); + ROS_INFO("ERROR: Failed to set transformation!!!\n"); + ROS_INFO("Sent angles: %f roll %f pitch %f yaw\n", angles[0], angles[1], angles[2]); + ROS_INFO("Returned angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); } res.success = true; return true; @@ -1308,16 +1476,20 @@ namespace Microstrain //Get sensor to vehicle frame transformation bool Microstrain::get_sensor_vehicle_frame_trans(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - if(GX5_15 == true){ + if(GX5_15 == true) + { ROS_INFO("Device does not support this feature"); res.success = false; return true; } + memset(readback_angles, 0, 3*sizeof(float)); //Read back the transformation start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } @@ -1329,9 +1501,9 @@ namespace Microstrain return true; } - //Set reference position - bool Microstrain::set_reference_position(microstrain_mips::SetReferencePosition::Request &req, microstrain_mips::SetReferencePosition::Response &res) - { + //Set reference position + bool Microstrain::set_reference_position(microstrain_mips::SetReferencePosition::Request &req, microstrain_mips::SetReferencePosition::Response &res) + { ROS_INFO("Setting reference Position\n"); memset(reference_position_command, 0, 3*sizeof(double)); @@ -1344,8 +1516,10 @@ namespace Microstrain reference_position_command[2] = req.position.z; start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &reference_position_enable_command, reference_position_command) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &reference_position_enable_command, reference_position_command) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_reference_position function timed out."); break; } @@ -1353,8 +1527,10 @@ namespace Microstrain //Read back the reference position start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_reference_position function timed out."); break; } @@ -1365,25 +1541,27 @@ namespace Microstrain (abs(reference_position_command[1] - reference_position_readback[1]) < 0.001) && (abs(reference_position_command[2] - reference_position_readback[2]) < 0.001)) { - ROS_INFO("Reference position successfully set\n"); + ROS_INFO("Reference position successfully set\n"); } else { - ROS_ERROR("Failed to set the reference position!!!\n"); + ROS_ERROR("Failed to set the reference position!!!\n"); } res.success = true; return true; - } + } - //Get reference position - bool Microstrain::get_reference_position(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { + //Get reference position + bool Microstrain::get_reference_position(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { ROS_INFO("Getting reference position"); memset(reference_position_readback, 0, 3*sizeof(double)); start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_reference_position function timed out."); break; } @@ -1392,17 +1570,20 @@ namespace Microstrain res.success = true; return true; - } + } - //Enable or disable coning and sculling compensation - bool Microstrain::set_coning_sculling_comp(microstrain_mips::SetConingScullingComp::Request &req, microstrain_mips::SetConingScullingComp::Response &res) + //Enable or disable coning and sculling compensation + bool Microstrain::set_coning_sculling_comp(microstrain_mips::SetConingScullingComp::Request &req, microstrain_mips::SetConingScullingComp::Response &res) + { + if(req.enable == 0) { - if(req.enable == 0){ ROS_INFO("Disabling Coning and Sculling compensation\n"); enable_flag = MIP_3DM_CONING_AND_SCULLING_DISABLE; start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } @@ -1410,21 +1591,25 @@ namespace Microstrain ROS_INFO("Reading Coning and Sculling compensation enabled state:\n"); start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } } ROS_INFO("%s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); - } - else if(req.enable == 1){ + else if(req.enable == 1) + { ROS_INFO("Enabling Coning and Sculling compensation\n"); enable_flag = MIP_3DM_CONING_AND_SCULLING_ENABLE; start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } @@ -1433,27 +1618,33 @@ namespace Microstrain //Read back enable/disable value ROS_INFO("Reading Coning and Sculling compensation enabled state:\n"); start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } } ROS_INFO("%s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); } - else{ + else + { ROS_INFO("Error: Input must be either 0 (disable) or 1 (enable)."); } + res.success = false; return true; - } + } - //Get coning and sculling compenastion enabled/disabled state - bool Microstrain::get_coning_sculling_comp(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { + //Get coning and sculling compenastion enabled/disabled state + bool Microstrain::get_coning_sculling_comp(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } @@ -1461,23 +1652,27 @@ namespace Microstrain ROS_INFO("Coning and Sculling compensation is: %s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); res.success = true; return true; - } + } - //Set estimation control filter flags - bool Microstrain::set_estimation_control_flags(microstrain_mips::SetEstimationControlFlags::Request &req, microstrain_mips::SetEstimationControlFlags::Response &res) - { + //Set estimation control filter flags + bool Microstrain::set_estimation_control_flags(microstrain_mips::SetEstimationControlFlags::Request &req, microstrain_mips::SetEstimationControlFlags::Response &res) + { estimation_control = req.flag; start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_estimation_control function timed out."); break; } } start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_estimation_control function timed out."); break; } @@ -1486,15 +1681,17 @@ namespace Microstrain res.success = true; return true; - } + } - //Get estimatio control filter flags - bool Microstrain::get_estimation_control_flags(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { + //Get estimatio control filter flags + bool Microstrain::get_estimation_control_flags(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ + while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { ROS_INFO("mip_filter_estimation_control function timed out."); break; } @@ -1503,1048 +1700,1130 @@ namespace Microstrain res.success = true; return true; - } + } - //Get device basic status. Variables in basic status struct change based on device model - bool Microstrain::get_basic_status(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Get device basic status. Variables in basic status struct change based on device model + bool Microstrain::get_basic_status(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + //Use the basic status struct for the GX-25 + if(GX5_25) { - //Use the basic status struct for the GX-25 - if(GX5_25){ - u8 response_buffer[sizeof(gx4_25_basic_status_field)]; - start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_BASIC_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); - break; - } + u8 response_buffer[sizeof(gx4_25_basic_status_field)]; + start = clock(); + while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_BASIC_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); + break; } - - ROS_INFO("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model); - ROS_INFO("Status Selector: \t\t\t\t%d\n", basic_field.status_selector);// == GX4_25_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report"); - ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", basic_field.status_flags); - ROS_INFO("System state: \t\t\t\t\t%04u\n", basic_field.system_state); - ROS_INFO("System Microsecond Timer Count: \t\t%lu ms\n\n", basic_field.system_timer_ms); - } - else { - ROS_INFO("Command not supported on this model"); } - res.success = true; - return true; + ROS_INFO("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model); + ROS_INFO("Status Selector: \t\t\t\t%d\n", basic_field.status_selector);// == GX4_25_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report"); + ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", basic_field.status_flags); + ROS_INFO("System state: \t\t\t\t\t%04u\n", basic_field.system_state); + ROS_INFO("System Microsecond Timer Count: \t\t%lu ms\n\n", basic_field.system_timer_ms); } - - //Get diagnostic status of device. Changes based on device model. - bool Microstrain::get_diagnostic_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) - { - //Use GX5-25 device diagnostic struct - if(GX5_25){ - u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; - start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); - break; - } - } - - ROS_INFO("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model); - ROS_INFO("Status Selector: \t\t\t\t%d\n", diagnostic_field.status_selector);// == 114 ? "Basic Status Report" : "Diagnostic Status Report"); - ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", diagnostic_field.status_flags); - ROS_INFO("System Millisecond Timer Count: \t\t%lu ms\n", diagnostic_field.system_timer_ms); - ROS_INFO("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE"); - ROS_INFO("FILTER Streaming Enabled: \t\t\t%s\n", diagnostic_field.filter_stream_enabled == 1 ? "TRUE" : "FALSE"); - ROS_INFO("Number of Dropped IMU Packets: \t\t\t%lu packets\n", diagnostic_field.imu_dropped_packets); - ROS_INFO("Number of Dropped FILTER Packets: \t\t%lu packets\n", diagnostic_field.filter_dropped_packets); - ROS_INFO("Communications Port Bytes Written: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_written); - ROS_INFO("Communications Port Bytes Read: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_read); - ROS_INFO("Communications Port Write Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_write_overruns); - ROS_INFO("Communications Port Read Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_read_overruns); - ROS_INFO("IMU Parser Errors: \t\t\t\t%lu Errors\n", diagnostic_field.imu_parser_errors); - ROS_INFO("IMU Message Count: \t\t\t\t%lu Messages\n", diagnostic_field.imu_message_count); - ROS_INFO("IMU Last Message Received: \t\t\t%lu ms\n", diagnostic_field.imu_last_message_ms); - - } - - //Unable to get this function working for the GX5-45 - else { - ROS_INFO("Command not supported on this model"); - } - - res.success = true; - return true; + else { + ROS_INFO("Command not supported on this model"); } - //Set zero angular-rate update threshold - bool Microstrain::set_zero_angle_update_threshold(microstrain_mips::SetZeroAngleUpdateThreshold::Request &req, microstrain_mips::SetZeroAngleUpdateThreshold::Response &res) - { - ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); - - zero_update_control.threshold = req.threshold; // rads/s - zero_update_control.enable = req.enable; //enable zero-angular-rate update - - //Set ZUPT parameters - start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); - break; - } - } + res.success = true; + return true; + } - //Read back parameter settings + //Get diagnostic status of device. Changes based on device model. + bool Microstrain::get_diagnostic_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + //Use GX5-25 device diagnostic struct + if(GX5_25) + { + u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); + while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); break; } } - if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold) - ROS_INFO("ERROR configuring Zero Angular Rate Update.\n"); + ROS_INFO("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model); + ROS_INFO("Status Selector: \t\t\t\t%d\n", diagnostic_field.status_selector);// == 114 ? "Basic Status Report" : "Diagnostic Status Report"); + ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", diagnostic_field.status_flags); + ROS_INFO("System Millisecond Timer Count: \t\t%lu ms\n", diagnostic_field.system_timer_ms); + ROS_INFO("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE"); + ROS_INFO("FILTER Streaming Enabled: \t\t\t%s\n", diagnostic_field.filter_stream_enabled == 1 ? "TRUE" : "FALSE"); + ROS_INFO("Number of Dropped IMU Packets: \t\t\t%lu packets\n", diagnostic_field.imu_dropped_packets); + ROS_INFO("Number of Dropped FILTER Packets: \t\t%lu packets\n", diagnostic_field.filter_dropped_packets); + ROS_INFO("Communications Port Bytes Written: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_written); + ROS_INFO("Communications Port Bytes Read: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_read); + ROS_INFO("Communications Port Write Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_write_overruns); + ROS_INFO("Communications Port Read Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_read_overruns); + ROS_INFO("IMU Parser Errors: \t\t\t\t%lu Errors\n", diagnostic_field.imu_parser_errors); + ROS_INFO("IMU Message Count: \t\t\t\t%lu Messages\n", diagnostic_field.imu_message_count); + ROS_INFO("IMU Last Message Received: \t\t\t%lu ms\n", diagnostic_field.imu_last_message_ms); + + } + //Unable to get this function working for the GX5-45 + else + { + ROS_INFO("Command not supported on this model"); + } - ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); + res.success = true; + return true; + } - res.success = true; - return true; - } + //Set zero angular-rate update threshold + bool Microstrain::set_zero_angle_update_threshold(microstrain_mips::SetZeroAngleUpdateThreshold::Request &req, microstrain_mips::SetZeroAngleUpdateThreshold::Response &res) + { + ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); + zero_update_control.threshold = req.threshold; // rads/s + zero_update_control.enable = req.enable; //enable zero-angular-rate update - //Get zero angular rate update threshold value - bool Microstrain::get_zero_angle_update_threshold(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Set ZUPT parameters + start = clock(); + while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK) { - ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); - //Read back parameter settings - - start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); + break; } - ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); - - res.success = true; - return true; } - //Set tare orientation angle values - bool Microstrain::set_tare_orientation(microstrain_mips::SetTareOrientation::Request &req, microstrain_mips::SetTareOrientation::Response &res) + //Read back parameter settings + start = clock(); + while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) { - if(req.axis < 1 || req.axis > 7){ - ROS_INFO("Value must be between 1-7. 1 = Roll, 2 = Pitch, 3 = Roll/Pitch, 4 = Yaw, 5 = Roll/Yaw, 6 = Pitch/Yaw, 7 = Roll/Pitch/Yaw"); - res.success = false; + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); + break; } + } - angles[0] = angles[1] = angles[2] = 0; - int i = req.axis; + if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold) + ROS_INFO("ERROR configuring Zero Angular Rate Update.\n"); - start = clock(); - while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_set_init_attitude function timed out."); - break; - } - } + ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); - //Wait for Filter to re-establish running state - Sleep(5000); - //Cycle through axes combinations - if(mip_filter_tare_orientation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK) - { - ROS_INFO("ERROR: Failed Axis - "); + res.success = true; + return true; + } - if(i & FILTER_TARE_ROLL_AXIS) - ROS_INFO(" Roll Axis "); - if(i & FILTER_TARE_PITCH_AXIS) - ROS_INFO(" Pitch Axis "); + //Get zero angular rate update threshold value + bool Microstrain::get_zero_angle_update_threshold(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); + //Read back parameter settings - if(i & FILTER_TARE_YAW_AXIS) - ROS_INFO(" Yaw Axis "); + start = clock(); + while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); + break; } - else + } + ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); + + res.success = true; + return true; + } + + //Set tare orientation angle values + bool Microstrain::set_tare_orientation(microstrain_mips::SetTareOrientation::Request &req, microstrain_mips::SetTareOrientation::Response &res) + { + if(req.axis < 1 || req.axis > 7) + { + ROS_INFO("Value must be between 1-7. 1 = Roll, 2 = Pitch, 3 = Roll/Pitch, 4 = Yaw, 5 = Roll/Yaw, 6 = Pitch/Yaw, 7 = Roll/Pitch/Yaw"); + res.success = false; + } + + angles[0] = angles[1] = angles[2] = 0; + int i = req.axis; + + start = clock(); + while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { + ROS_INFO("mip_filter_set_init_attitude function timed out."); + break; + } + } + + //Wait for Filter to re-establish running state + Sleep(5000); + //Cycle through axes combinations + if(mip_filter_tare_orientation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK) + { + ROS_INFO("ERROR: Failed Axis - "); + + if(i & FILTER_TARE_ROLL_AXIS) + ROS_INFO(" Roll Axis "); + + if(i & FILTER_TARE_PITCH_AXIS) + ROS_INFO(" Pitch Axis "); + + if(i & FILTER_TARE_YAW_AXIS) + ROS_INFO(" Yaw Axis "); + } + else + { ROS_INFO("Tare Configuration = %d\n", i); ROS_INFO("Tared -"); if(i & FILTER_TARE_ROLL_AXIS) - ROS_INFO(" Roll Axis "); + ROS_INFO(" Roll Axis "); if(i & FILTER_TARE_PITCH_AXIS) - ROS_INFO(" Pitch Axis "); + ROS_INFO(" Pitch Axis "); if(i & FILTER_TARE_YAW_AXIS) - ROS_INFO(" Yaw Axis "); + ROS_INFO(" Yaw Axis "); res.success = true; return true; } Sleep(1000); - } - - //Set accel noise values - bool Microstrain::set_accel_noise(microstrain_mips::SetAccelNoise::Request &req, microstrain_mips::SetAccelNoise::Response &res) - { - ROS_INFO("Setting the accel noise values\n"); - - noise[0] = req.noise.x; - noise[1] = req.noise.y; - noise[2] = req.noise.z; + } - start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_noise function timed out."); - break; - } - } + //Set accel noise values + bool Microstrain::set_accel_noise(microstrain_mips::SetAccelNoise::Request &req, microstrain_mips::SetAccelNoise::Response &res) + { + ROS_INFO("Setting the accel noise values\n"); - //Read back the accel noise values - start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_noise function timed out."); - break; - } - } + noise[0] = req.noise.x; + noise[1] = req.noise.y; + noise[2] = req.noise.z; - if((abs(readback_noise[0]-noise[0]) < 0.001) && - (abs(readback_noise[1]-noise[1]) < 0.001) && - (abs(readback_noise[2]-noise[2]) < 0.001)) - { - ROS_INFO("Accel noise values successfully set.\n"); - } - else + start = clock(); + while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set accel noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("mip_filter_accel_noise function timed out."); + break; } - - res.success; - return true; } - //Get accel noise values - bool Microstrain::get_accel_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Read back the accel noise values + start = clock(); + while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_noise function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_noise function timed out."); + break; } - ROS_INFO("Accel noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); - - res.success = true; - return true; } - //Set gyro noise values - bool Microstrain::set_gyro_noise(microstrain_mips::SetGyroNoise::Request &req, microstrain_mips::SetGyroNoise::Response &res) + if((abs(readback_noise[0]-noise[0]) < 0.001) && + (abs(readback_noise[1]-noise[1]) < 0.001) && + (abs(readback_noise[2]-noise[2]) < 0.001)) { - ROS_INFO("Setting the gyro noise values\n"); + ROS_INFO("Accel noise values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set accel noise values!!!\n"); + ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + } - noise[0] = req.noise.x; - noise[1] = req.noise.y; - noise[2] = req.noise.z; + res.success; + return true; + } - start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_noise function timed out."); - break; - } + //Get accel noise values + bool Microstrain::get_accel_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + start = clock(); + while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_noise function timed out."); + break; } + } + ROS_INFO("Accel noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); - //Read back the gyro noise values - start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_noise function timed out."); - break; - } - } + res.success = true; + return true; + } - if((abs(readback_noise[0]-noise[0]) < 0.001) && - (abs(readback_noise[1]-noise[1]) < 0.001) && - (abs(readback_noise[2]-noise[2]) < 0.001)) - { - ROS_INFO("Gyro noise values successfully set.\n"); - } - else + //Set gyro noise values + bool Microstrain::set_gyro_noise(microstrain_mips::SetGyroNoise::Request &req, microstrain_mips::SetGyroNoise::Response &res) + { + ROS_INFO("Setting the gyro noise values\n"); + + noise[0] = req.noise.x; + noise[1] = req.noise.y; + noise[2] = req.noise.z; + + start = clock(); + while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set gyro noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("mip_filter_gyro_noise function timed out."); + break; } - - res.success = true; - return true; } - //Get gyro noise values - bool Microstrain::get_gyro_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Read back the gyro noise values + start = clock(); + while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_noise function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_gyro_noise function timed out."); + break; } - ROS_INFO("Gyro noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + } - res.success = true; - return true; + if((abs(readback_noise[0]-noise[0]) < 0.001) && + (abs(readback_noise[1]-noise[1]) < 0.001) && + (abs(readback_noise[2]-noise[2]) < 0.001)) + { + ROS_INFO("Gyro noise values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set gyro noise values!!!\n"); + ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); } - //Set magnetometer noise values - bool Microstrain::set_mag_noise(microstrain_mips::SetMagNoise::Request &req, microstrain_mips::SetMagNoise::Response &res) + res.success = true; + return true; + } + + //Get gyro noise values + bool Microstrain::get_gyro_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + start = clock(); + while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_gyro_noise function timed out."); + break; } + } + ROS_INFO("Gyro noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); - ROS_INFO("Setting the mag noise values\n"); + res.success = true; + return true; + } - noise[0] = req.noise.x; - noise[1] = req.noise.y; - noise[2] = req.noise.z; + //Set magnetometer noise values + bool Microstrain::set_mag_noise(microstrain_mips::SetMagNoise::Request &req, microstrain_mips::SetMagNoise::Response &res) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_noise function timed out."); - break; - } - } + ROS_INFO("Setting the mag noise values\n"); - //Read back the mag white noise values - start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_noise function timed out."); - break; - } - } + noise[0] = req.noise.x; + noise[1] = req.noise.y; + noise[2] = req.noise.z; - if((abs(readback_noise[0] - noise[0]) < 0.001) && - (abs(readback_noise[1] - noise[1]) < 0.001) && - (abs(readback_noise[2] - noise[2]) < 0.001)) - { - ROS_INFO("Mag noise values successfully set.\n"); - } - else + start = clock(); + while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set mag noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("mip_filter_mag_noise function timed out."); + break; } - - res.success = true; - return true; } - //Get magnetometer noise values - bool Microstrain::get_mag_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Read back the mag white noise values + start = clock(); + while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_mag_noise function timed out."); + break; } + } - start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_noise function timed out."); - break; - } - } + if((abs(readback_noise[0] - noise[0]) < 0.001) && + (abs(readback_noise[1] - noise[1]) < 0.001) && + (abs(readback_noise[2] - noise[2]) < 0.001)) + { + ROS_INFO("Mag noise values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set mag noise values!!!\n"); + ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + } - res.success = true; + res.success = true; + return true; + } + + //Get magnetometer noise values + bool Microstrain::get_mag_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } - - //Set gyro bias model - bool Microstrain::set_gyro_bias_model(microstrain_mips::SetGyroBiasModel::Request &req, microstrain_mips::SetGyroBiasModel::Response &res) + start = clock(); + while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - ROS_INFO("Setting the gyro bias model values\n"); + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_mag_noise function timed out."); + break; + } + } + ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); - noise[0] = req.noise_vector.x; - noise[1] = req.noise_vector.y; - noise[2] = req.noise_vector.z; + res.success = true; + return true; + } - beta[0] = req.beta_vector.x; - beta[1] = req.beta_vector.x; - beta[2] = req.beta_vector.x; - start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_bias_model function timed out."); - break; - } - } + //Set gyro bias model + bool Microstrain::set_gyro_bias_model(microstrain_mips::SetGyroBiasModel::Request &req, microstrain_mips::SetGyroBiasModel::Response &res) + { + ROS_INFO("Setting the gyro bias model values\n"); - //Read back the gyro bias model values - start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_bias_model function timed out."); - break; - } - } + noise[0] = req.noise_vector.x; + noise[1] = req.noise_vector.y; + noise[2] = req.noise_vector.z; + + beta[0] = req.beta_vector.x; + beta[1] = req.beta_vector.x; + beta[2] = req.beta_vector.x; - if((abs(readback_noise[0]-noise[0]) < 0.001) && - (abs(readback_noise[1]-noise[1]) < 0.001) && - (abs(readback_noise[2]-noise[2]) < 0.001) && - (abs(readback_beta[0]-beta[0]) < 0.001) && - (abs(readback_beta[1]-beta[1]) < 0.001) && - (abs(readback_beta[2]-beta[2]) < 0.001)) + start = clock(); + while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("Gyro bias model values successfully set.\n"); + ROS_INFO("mip_filter_gyro_bias_model function timed out."); + break; } - else + } + + //Read back the gyro bias model values + start = clock(); + while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set gyro bias model values!!!\n"); - ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], - readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("mip_filter_gyro_bias_model function timed out."); + break; } + } - res.success = true; - return true; + if((abs(readback_noise[0]-noise[0]) < 0.001) && + (abs(readback_noise[1]-noise[1]) < 0.001) && + (abs(readback_noise[2]-noise[2]) < 0.001) && + (abs(readback_beta[0]-beta[0]) < 0.001) && + (abs(readback_beta[1]-beta[1]) < 0.001) && + (abs(readback_beta[2]-beta[2]) < 0.001)) + { + ROS_INFO("Gyro bias model values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set gyro bias model values!!!\n"); + ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], + readback_noise[0], readback_noise[1], readback_noise[2]); } - //Get gyro bias model - bool Microstrain::get_gyro_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + res.success = true; + return true; + } + + //Get gyro bias model + bool Microstrain::get_gyro_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + start = clock(); + while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) { - start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_gyro_bias_model function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_gyro_bias_model function timed out."); + break; } + } - ROS_INFO("Gyro bias model values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]); - res.success = true; + ROS_INFO("Gyro bias model values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]); + res.success = true; + return true; + } + + //Get acces bias model + bool Microstrain::get_accel_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + if(GX5_15 == true || GX5_25 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } - //Get acces bias model - bool Microstrain::get_accel_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + memset(readback_noise, 0, 3*sizeof(float)); + memset(readback_beta, 0, 3*sizeof(float)); + + start = clock(); + while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if (clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_bias_model function timed out."); + break; } + } - memset(readback_noise, 0, 3*sizeof(float)); - memset(readback_beta, 0, 3*sizeof(float)); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], + readback_noise[0], readback_noise[1], readback_noise[2]); - start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_bias_model function timed out."); - break; - } - } - - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], - readback_noise[0], readback_noise[1], readback_noise[2]); + res.success = true; + return true; + } - res.success = true; + //Set accel bias model + bool Microstrain::set_accel_bias_model(microstrain_mips::SetAccelBiasModel::Request &req, microstrain_mips::SetAccelBiasModel::Response &res) + { + if(GX5_15 == true || GX5_25 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } + memset(noise, 0, 3*sizeof(float)); + memset(beta, 0, 3*sizeof(float)); + memset(readback_noise, 0, 3*sizeof(float)); + memset(readback_beta, 0, 3*sizeof(float)); + ROS_INFO("Setting the accel bias model values\n"); + + noise[0] = req.noise_vector.x; + noise[1] = req.noise_vector.y; + noise[2] = req.noise_vector.z; - //Set accel bias model - bool Microstrain::set_accel_bias_model(microstrain_mips::SetAccelBiasModel::Request &req, microstrain_mips::SetAccelBiasModel::Response &res) + beta[0] = req.beta_vector.x; + beta[1] = req.beta_vector.x; + beta[2] = req.beta_vector.x; + + start = clock(); + while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_bias_model function timed out."); + break; } + } - memset(noise, 0, 3*sizeof(float)); - memset(beta, 0, 3*sizeof(float)); - memset(readback_noise, 0, 3*sizeof(float)); - memset(readback_beta, 0, 3*sizeof(float)); - ROS_INFO("Setting the accel bias model values\n"); + //Read back the accel bias model values + start = clock(); + while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_bias_model function timed out."); + break; + } + } - noise[0] = req.noise_vector.x; - noise[1] = req.noise_vector.y; - noise[2] = req.noise_vector.z; + if((abs(readback_noise[0]-noise[0]) < 0.001) && + (abs(readback_noise[1]-noise[1]) < 0.001) && + (abs(readback_noise[2]-noise[2]) < 0.001) && + (abs(readback_beta[0]-beta[0]) < 0.001) && + (abs(readback_beta[1]-beta[1]) < 0.001) && + (abs(readback_beta[2]-beta[2]) < 0.001)) + { + ROS_INFO("Accel bias model values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set accel bias model values!!!\n"); + ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], + readback_noise[0], readback_noise[1], readback_noise[2]); + } - beta[0] = req.beta_vector.x; - beta[1] = req.beta_vector.x; - beta[2] = req.beta_vector.x; + res.success = true; + return true; - start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_bias_model function timed out."); - break; - } - } + } - //Read back the accel bias model values - start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_bias_model function timed out."); - break; - } - } + //Set accel magnitude error adaptive measurement values + bool Microstrain::set_accel_adaptive_vals(microstrain_mips::SetAccelAdaptiveVals::Request &req, microstrain_mips::SetAccelAdaptiveVals::Response &res ) + { + ROS_INFO("Setting the accel magnitude error adaptive measurement values\n"); - if((abs(readback_noise[0]-noise[0]) < 0.001) && - (abs(readback_noise[1]-noise[1]) < 0.001) && - (abs(readback_noise[2]-noise[2]) < 0.001) && - (abs(readback_beta[0]-beta[0]) < 0.001) && - (abs(readback_beta[1]-beta[1]) < 0.001) && - (abs(readback_beta[2]-beta[2]) < 0.001)) + accel_magnitude_error_command.enable = req.enable; + accel_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff; + accel_magnitude_error_command.min_1sigma = req.min_1sigma; + accel_magnitude_error_command.low_limit = req.low_limit; + accel_magnitude_error_command.high_limit = req.high_limit; + accel_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma; + accel_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; + + start = clock(); + while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("Accel bias model values successfully set.\n"); + ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); + break; } - else + } + + //Read back the accel magnitude error adaptive measurement values + start = clock(); + while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set accel bias model values!!!\n"); - ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], - readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); + break; } - - res.success = true; - return true; - } - //Set accel magnitude error adaptive measurement values - bool Microstrain::set_accel_adaptive_vals(microstrain_mips::SetAccelAdaptiveVals::Request &req, microstrain_mips::SetAccelAdaptiveVals::Response &res ) + if((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) && + (abs(accel_magnitude_error_command.low_pass_cutoff - accel_magnitude_error_readback.low_pass_cutoff) < 0.001) && + (abs(accel_magnitude_error_command.min_1sigma - accel_magnitude_error_readback.min_1sigma) < 0.001) && + (abs(accel_magnitude_error_command.low_limit - accel_magnitude_error_readback.low_limit) < 0.001) && + (abs(accel_magnitude_error_command.high_limit - accel_magnitude_error_readback.high_limit) < 0.001) && + (abs(accel_magnitude_error_command.low_limit_1sigma - accel_magnitude_error_readback.low_limit_1sigma) < 0.001) && + (abs(accel_magnitude_error_command.high_limit_1sigma - accel_magnitude_error_readback.high_limit_1sigma) < 0.001)) + { + ROS_INFO("accel magnitude error adaptive measurement values successfully set.\n"); + } + else { - ROS_INFO("Setting the accel magnitude error adaptive measurement values\n"); + ROS_INFO("ERROR: Failed to set accel magnitude error adaptive measurement values!!!"); + ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_command.enable, accel_magnitude_error_command.low_pass_cutoff, accel_magnitude_error_command.min_1sigma, accel_magnitude_error_command.low_limit, accel_magnitude_error_command.high_limit, accel_magnitude_error_command.low_limit_1sigma, accel_magnitude_error_command.high_limit_1sigma); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + } - accel_magnitude_error_command.enable = req.enable; - accel_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff; - accel_magnitude_error_command.min_1sigma = req.min_1sigma; - accel_magnitude_error_command.low_limit = req.low_limit; - accel_magnitude_error_command.high_limit = req.high_limit; - accel_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma; - accel_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; + res.success = true; + return true; - start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + } - //Read back the accel magnitude error adaptive measurement values - start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); - break; - } + //Get accep magnitude error adaptive measurement values + bool Microstrain::get_accel_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) + { + start = clock(); + while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); + break; } + } + ROS_INFO("Accel magnitude error adaptive measurement values are: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + + res.success = true; + return true; + } + + //Set magnetometer magnitude error adaptive measurement values + bool Microstrain::set_mag_adaptive_vals(microstrain_mips::SetMagAdaptiveVals::Request &req, microstrain_mips::SetMagAdaptiveVals::Response &res ) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } + + ROS_INFO("Setting the mag magnitude error adaptive measurement values\n"); - if((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) && - (abs(accel_magnitude_error_command.low_pass_cutoff - accel_magnitude_error_readback.low_pass_cutoff) < 0.001) && - (abs(accel_magnitude_error_command.min_1sigma - accel_magnitude_error_readback.min_1sigma) < 0.001) && - (abs(accel_magnitude_error_command.low_limit - accel_magnitude_error_readback.low_limit) < 0.001) && - (abs(accel_magnitude_error_command.high_limit - accel_magnitude_error_readback.high_limit) < 0.001) && - (abs(accel_magnitude_error_command.low_limit_1sigma - accel_magnitude_error_readback.low_limit_1sigma) < 0.001) && - (abs(accel_magnitude_error_command.high_limit_1sigma - accel_magnitude_error_readback.high_limit_1sigma) < 0.001)) + mag_magnitude_error_command.enable = req.enable; + mag_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff; + mag_magnitude_error_command.min_1sigma = req.min_1sigma; + mag_magnitude_error_command.low_limit = req.low_limit; + mag_magnitude_error_command.high_limit = req.high_limit; + mag_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma; + mag_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; + + start = clock(); + while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_magnitude_error_command) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("accel magnitude error adaptive measurement values successfully set.\n"); + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } - else + } + + //Read back the mag magnitude error adaptive measurement values + start = clock(); + while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set accel magnitude error adaptive measurement values!!!"); - ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_command.enable, accel_magnitude_error_command.low_pass_cutoff, accel_magnitude_error_command.min_1sigma, accel_magnitude_error_command.low_limit, accel_magnitude_error_command.high_limit, accel_magnitude_error_command.low_limit_1sigma, accel_magnitude_error_command.high_limit_1sigma); - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } + } - res.success = true; - return true; + if((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) && + (abs(mag_magnitude_error_command.low_pass_cutoff - mag_magnitude_error_readback.low_pass_cutoff) < 0.001) && + (abs(mag_magnitude_error_command.min_1sigma - mag_magnitude_error_readback.min_1sigma) < 0.001) && + (abs(mag_magnitude_error_command.low_limit - mag_magnitude_error_readback.low_limit) < 0.001) && + (abs(mag_magnitude_error_command.high_limit - mag_magnitude_error_readback.high_limit) < 0.001) && + (abs(mag_magnitude_error_command.low_limit_1sigma - mag_magnitude_error_readback.low_limit_1sigma) < 0.001) && + (abs(mag_magnitude_error_command.high_limit_1sigma - mag_magnitude_error_readback.high_limit_1sigma) < 0.001)) + { + ROS_INFO("mag magnitude error adaptive measurement values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n"); + ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_command.enable, mag_magnitude_error_command.low_pass_cutoff, mag_magnitude_error_command.min_1sigma, mag_magnitude_error_command.low_limit, mag_magnitude_error_command.high_limit, mag_magnitude_error_command.low_limit_1sigma, mag_magnitude_error_command.high_limit_1sigma); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); + } + res.success = true; + return true; + } + + //Get magnetometer magnitude error adaptive measurement values + bool Microstrain::get_mag_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) + { + if(GX5_15 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; } - //Get accep magnitude error adaptive measurement values - bool Microstrain::get_accel_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) + start = clock(); + while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) { - start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); - break; - } + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } - ROS_INFO("Accel magnitude error adaptive measurement values are: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + } - res.success = true; + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); + res.success = true; + return true; + } + + //Get magnetometer dip angle error adaptive measurement values + bool Microstrain::get_mag_dip_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) + { + if(GX5_15 == true || GX5_25 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } - //Set magnetometer magnitude error adaptive measurement values - bool Microstrain::set_mag_adaptive_vals(microstrain_mips::SetMagAdaptiveVals::Request &req, microstrain_mips::SetMagAdaptiveVals::Response &res ) + start = clock(); + while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } + } - ROS_INFO("Setting the mag magnitude error adaptive measurement values\n"); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable, + mag_dip_angle_error_readback.low_pass_cutoff, + mag_dip_angle_error_readback.min_1sigma, + mag_dip_angle_error_readback.high_limit, + mag_dip_angle_error_readback.high_limit_1sigma); + + res.success = true; + return true; + } - mag_magnitude_error_command.enable = req.enable; - mag_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff; - mag_magnitude_error_command.min_1sigma = req.min_1sigma; - mag_magnitude_error_command.low_limit = req.low_limit; - mag_magnitude_error_command.high_limit = req.high_limit; - mag_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma; - mag_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; + //Get magnetometer dip angle error adaptive measurement values + bool Microstrain::set_mag_dip_adaptive_vals(microstrain_mips::SetMagDipAdaptiveVals::Request &req, microstrain_mips::SetMagDipAdaptiveVals::Response &res ) + { + if(GX5_15 == true || GX5_25 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_magnitude_error_command) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + ROS_INFO("Setting the mag dip angle error adaptive measurement values\n"); - //Read back the mag magnitude error adaptive measurement values - start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + mag_dip_angle_error_command.enable = req.enable; + mag_dip_angle_error_command.low_pass_cutoff = req.low_pass_cutoff; + mag_dip_angle_error_command.min_1sigma = req.min_1sigma; + mag_dip_angle_error_command.high_limit = req.high_limit; + mag_dip_angle_error_command.high_limit_1sigma = req.high_limit_1sigma; - if((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) && - (abs(mag_magnitude_error_command.low_pass_cutoff - mag_magnitude_error_readback.low_pass_cutoff) < 0.001) && - (abs(mag_magnitude_error_command.min_1sigma - mag_magnitude_error_readback.min_1sigma) < 0.001) && - (abs(mag_magnitude_error_command.low_limit - mag_magnitude_error_readback.low_limit) < 0.001) && - (abs(mag_magnitude_error_command.high_limit - mag_magnitude_error_readback.high_limit) < 0.001) && - (abs(mag_magnitude_error_command.low_limit_1sigma - mag_magnitude_error_readback.low_limit_1sigma) < 0.001) && - (abs(mag_magnitude_error_command.high_limit_1sigma - mag_magnitude_error_readback.high_limit_1sigma) < 0.001)) + start = clock(); + while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("mag magnitude error adaptive measurement values successfully set.\n"); + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } - else + } + + //Read back the mag magnitude error adaptive measurement values + start = clock(); + while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n"); - ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_command.enable, mag_magnitude_error_command.low_pass_cutoff, mag_magnitude_error_command.min_1sigma, mag_magnitude_error_command.low_limit, mag_magnitude_error_command.high_limit, mag_magnitude_error_command.low_limit_1sigma, mag_magnitude_error_command.high_limit_1sigma); - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + break; } - - res.success = true; - return true; } - //Get magnetometer magnitude error adaptive measurement values - bool Microstrain::get_mag_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) + if((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) && + (abs(mag_dip_angle_error_command.low_pass_cutoff - mag_dip_angle_error_readback.low_pass_cutoff) < 0.001) && + (abs(mag_dip_angle_error_command.min_1sigma - mag_dip_angle_error_readback.min_1sigma) < 0.001) && + (abs(mag_dip_angle_error_command.high_limit - mag_dip_angle_error_readback.high_limit) < 0.001) && + (abs(mag_dip_angle_error_command.high_limit_1sigma - mag_dip_angle_error_readback.high_limit_1sigma) < 0.001)) { - if(GX5_15 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } + ROS_INFO("mag dip angle error adaptive measurement values successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set mag dip angle error adaptive measurement values!!!\n"); + ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_command.enable, + mag_dip_angle_error_command.low_pass_cutoff, + mag_dip_angle_error_command.min_1sigma, + mag_dip_angle_error_command.high_limit, + mag_dip_angle_error_command.high_limit_1sigma); - start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable, + mag_dip_angle_error_readback.low_pass_cutoff, + mag_dip_angle_error_readback.min_1sigma, + mag_dip_angle_error_readback.high_limit, + mag_dip_angle_error_readback.high_limit_1sigma); + } - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); - res.success = true; - return true; + res.success = true; + return true; + } + //Get vehicle dynamics mode + bool Microstrain::get_dynamics_mode(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + if(GX5_15 == true || GX5_25 == true) + { + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; } - //Get magnetometer dip angle error adaptive measurement values - bool Microstrain::get_mag_dip_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) - { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } + readback_dynamics_mode = 0; + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} - start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + ROS_INFO("Vehicle dynamics mode is: %d\n", dynamics_mode); - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable, - mag_dip_angle_error_readback.low_pass_cutoff, - mag_dip_angle_error_readback.min_1sigma, - mag_dip_angle_error_readback.high_limit, - mag_dip_angle_error_readback.high_limit_1sigma); + res.success = true; + return true; + } - res.success = true; - return true; - } - //Get magnetometer dip angle error adaptive measurement values - bool Microstrain::set_mag_dip_adaptive_vals(microstrain_mips::SetMagDipAdaptiveVals::Request &req, microstrain_mips::SetMagDipAdaptiveVals::Response &res ) + //Set vehicle dynamics mode. Only in 45 model. + bool Microstrain::set_dynamics_mode(microstrain_mips::SetDynamicsMode::Request &req, microstrain_mips::SetDynamicsMode::Response &res) + { + if(GX5_15 == true || GX5_25 == true) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } - - ROS_INFO("Setting the mag dip angle error adaptive measurement values\n"); + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - mag_dip_angle_error_command.enable = req.enable; - mag_dip_angle_error_command.low_pass_cutoff = req.low_pass_cutoff; - mag_dip_angle_error_command.min_1sigma = req.min_1sigma; - mag_dip_angle_error_command.high_limit = req.high_limit; - mag_dip_angle_error_command.high_limit_1sigma = req.high_limit_1sigma; + dynamics_mode = req.mode; + if (dynamics_mode < 1 || dynamics_mode > 3) + { + ROS_INFO("Error: Vehicle dynamics mode must be between 1-3"); + res.success = false; + } + else + { start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); break; } } - //Read back the mag magnitude error adaptive measurement values - start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); - break; - } - } + readback_dynamics_mode = 0; + while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} - if((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) && - (abs(mag_dip_angle_error_command.low_pass_cutoff - mag_dip_angle_error_readback.low_pass_cutoff) < 0.001) && - (abs(mag_dip_angle_error_command.min_1sigma - mag_dip_angle_error_readback.min_1sigma) < 0.001) && - (abs(mag_dip_angle_error_command.high_limit - mag_dip_angle_error_readback.high_limit) < 0.001) && - (abs(mag_dip_angle_error_command.high_limit_1sigma - mag_dip_angle_error_readback.high_limit_1sigma) < 0.001)) + if(dynamics_mode == readback_dynamics_mode) { - ROS_INFO("mag dip angle error adaptive measurement values successfully set.\n"); + ROS_INFO("Vehicle dynamics mode successfully set to %d\n", dynamics_mode); + res.success = true; } else { - ROS_INFO("ERROR: Failed to set mag dip angle error adaptive measurement values!!!\n"); - ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_command.enable, - mag_dip_angle_error_command.low_pass_cutoff, - mag_dip_angle_error_command.min_1sigma, - mag_dip_angle_error_command.high_limit, - mag_dip_angle_error_command.high_limit_1sigma); - - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable, - mag_dip_angle_error_readback.low_pass_cutoff, - mag_dip_angle_error_readback.min_1sigma, - mag_dip_angle_error_readback.high_limit, - mag_dip_angle_error_readback.high_limit_1sigma); + ROS_INFO("ERROR: Failed to set vehicle dynamics mode to %d!!!\n", dynamics_mode); + res.success = false; } - - res.success = true; - return true; } + return true; + } - - - //Get vehicle dynamics mode - bool Microstrain::get_dynamics_mode(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + //Set sensor to vehicle frame offset. Only in 45 + bool Microstrain::set_sensor_vehicle_frame_offset(microstrain_mips::SetSensorVehicleFrameOffset::Request &req, microstrain_mips::SetSensorVehicleFrameOffset::Response &res) + { + if(GX5_15 == true || GX5_25 == true) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } - - readback_dynamics_mode = 0; - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} - - ROS_INFO("Vehicle dynamics mode is: %d\n", dynamics_mode); - - res.success = true; + ROS_INFO("Device does not support this feature"); + res.success = false; return true; } + memset(offset, 0, 3*sizeof(float)); + memset(readback_offset, 0, 3*sizeof(float)); + ROS_INFO("Setting the sensor to vehicle frame offset\n"); - //Set vehicle dynamics mode. Only in 45 model. - bool Microstrain::set_dynamics_mode(microstrain_mips::SetDynamicsMode::Request &req, microstrain_mips::SetDynamicsMode::Response &res) - { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } - - dynamics_mode = req.mode; - - if (dynamics_mode < 1 || dynamics_mode > 3){ - ROS_INFO("Error: Vehicle dynamics mode must be between 1-3"); - res.success = false; - } - else{ - start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); - break; - } - } - - readback_dynamics_mode = 0; - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} + offset[0] = req.offset.x; + offset[1] = req.offset.y; + offset[2] = req.offset.z; - if(dynamics_mode == readback_dynamics_mode) - { - ROS_INFO("Vehicle dynamics mode successfully set to %d\n", dynamics_mode); - res.success = true; - } - else - { - ROS_INFO("ERROR: Failed to set vehicle dynamics mode to %d!!!\n", dynamics_mode); - res.success = false; - } + start = clock(); + while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); + break; } - return true; } - //Set sensor to vehicle frame offset. Only in 45 - bool Microstrain::set_sensor_vehicle_frame_offset(microstrain_mips::SetSensorVehicleFrameOffset::Request &req, microstrain_mips::SetSensorVehicleFrameOffset::Response &res) + //Read back the transformation + start = clock(); + while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } - - memset(offset, 0, 3*sizeof(float)); - memset(readback_offset, 0, 3*sizeof(float)); - ROS_INFO("Setting the sensor to vehicle frame offset\n"); - - offset[0] = req.offset.x; - offset[1] = req.offset.y; - offset[2] = req.offset.z; - - start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); - break; - } - } - - //Read back the transformation - start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); - break; - } - } - - if((abs(readback_offset[0]-offset[0]) < 0.001) && - (abs(readback_offset[1]-offset[1]) < 0.001) && - (abs(readback_offset[2]-offset[2]) < 0.001)) - { - ROS_INFO("Offset successfully set.\n"); - } - else + if(clock() - start > 5000) { - ROS_INFO("ERROR: Failed to set offset!!!\n"); - ROS_INFO("Sent offset: %f X %f Y %f Z\n", offset[0], offset[1], offset[2]); - ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]); + ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); + break; } + } - res.success = true; - return true; + if((abs(readback_offset[0]-offset[0]) < 0.001) && + (abs(readback_offset[1]-offset[1]) < 0.001) && + (abs(readback_offset[2]-offset[2]) < 0.001)) + { + ROS_INFO("Offset successfully set.\n"); + } + else + { + ROS_INFO("ERROR: Failed to set offset!!!\n"); + ROS_INFO("Sent offset: %f X %f Y %f Z\n", offset[0], offset[1], offset[2]); + ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]); } - //Get sensor to vehicle frame offset. Only in 45 model. - bool Microstrain::get_sensor_vehicle_frame_offset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + res.success = true; + return true; + } + + //Get sensor to vehicle frame offset. Only in 45 model. + bool Microstrain::get_sensor_vehicle_frame_offset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + { + if(GX5_15 == true || GX5_25 == true) { - if(GX5_15 == true || GX5_25 == true){ - ROS_INFO("Device does not support this feature"); - res.success = false; - return true; - } + ROS_INFO("Device does not support this feature"); + res.success = false; + return true; + } - memset(readback_offset, 0, 3*sizeof(float)); + memset(readback_offset, 0, 3*sizeof(float)); - start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); - break; - } + start = clock(); + while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); + break; } + } - ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]); + ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]); - res.success = true; - return true; - } + res.success = true; + return true; + } - //Get basic or diagnostic status of device. Called by basic and diagnostic services. - u16 Microstrain::mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer) + //Get basic or diagnostic status of device. Called by basic and diagnostic services. + u16 Microstrain::mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer) + { + int total_size = 0; + if(GX5_25) { - int total_size = 0; - if(GX5_25) - { - gx4_25_basic_status_field *basic_ptr; - gx4_25_diagnostic_device_status_field *diagnostic_ptr; - u16 response_size = MIP_FIELD_HEADER_SIZE; + gx4_25_basic_status_field *basic_ptr; + gx4_25_diagnostic_device_status_field *diagnostic_ptr; + u16 response_size = MIP_FIELD_HEADER_SIZE; - //Set response size based on device model and whether basic or diagnostic status is chosen - if(status_selector == GX4_25_BASIC_STATUS_SEL) + //Set response size based on device model and whether basic or diagnostic status is chosen + if(status_selector == GX4_25_BASIC_STATUS_SEL) response_size += sizeof(gx4_25_basic_status_field); - else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) + else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) response_size += sizeof(gx4_25_diagnostic_device_status_field); - while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){} + while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){} - if(status_selector == GX4_25_BASIC_STATUS_SEL) - { - if(response_size != sizeof(gx4_25_basic_status_field)){ - return MIP_INTERFACE_ERROR; - } - else if(MIP_SDK_CONFIG_BYTESWAP){ - - //Perform byteswapping - byteswap_inplace(&response_buffer[0], sizeof(basic_field.device_model)); - byteswap_inplace(&response_buffer[2], sizeof(basic_field.status_selector)); - byteswap_inplace(&response_buffer[3], sizeof(basic_field.status_flags)); - byteswap_inplace(&response_buffer[7], sizeof(basic_field.system_state)); - byteswap_inplace(&response_buffer[9], sizeof(basic_field.system_timer_ms)); + if(status_selector == GX4_25_BASIC_STATUS_SEL) + { + if(response_size != sizeof(gx4_25_basic_status_field)) + { + return MIP_INTERFACE_ERROR; } - void * struct_pointer; - struct_pointer = &basic_field; + else if(MIP_SDK_CONFIG_BYTESWAP) + { - //Copy response from response buffer to basic status struct - memcpy(struct_pointer, response_buffer, sizeof(basic_field.device_model)); - memcpy((struct_pointer+2), &(response_buffer[2]), sizeof(basic_field.status_selector)); - memcpy((struct_pointer+3), &(response_buffer[3]), sizeof(basic_field.status_flags)); - memcpy((struct_pointer+7), &(response_buffer[7]), sizeof(basic_field.system_state)); - memcpy((struct_pointer+9), &(response_buffer[9]), sizeof(basic_field.system_timer_ms)); + //Perform byteswapping + byteswap_inplace(&response_buffer[0], sizeof(basic_field.device_model)); + byteswap_inplace(&response_buffer[2], sizeof(basic_field.status_selector)); + byteswap_inplace(&response_buffer[3], sizeof(basic_field.status_flags)); + byteswap_inplace(&response_buffer[7], sizeof(basic_field.system_state)); + byteswap_inplace(&response_buffer[9], sizeof(basic_field.system_timer_ms)); + } + void * struct_pointer; + struct_pointer = &basic_field; + //Copy response from response buffer to basic status struct + memcpy(struct_pointer, response_buffer, sizeof(basic_field.device_model)); + memcpy((struct_pointer+2), &(response_buffer[2]), sizeof(basic_field.status_selector)); + memcpy((struct_pointer+3), &(response_buffer[3]), sizeof(basic_field.status_flags)); + memcpy((struct_pointer+7), &(response_buffer[7]), sizeof(basic_field.system_state)); + memcpy((struct_pointer+9), &(response_buffer[9]), sizeof(basic_field.system_timer_ms)); - } - else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) - { + } + else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) + { - if(response_size != sizeof(gx4_25_diagnostic_device_status_field)){ - return MIP_INTERFACE_ERROR; - } - else if(MIP_SDK_CONFIG_BYTESWAP){ - - //byteswap and copy response to diagnostic status struct - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.device_model)); - total_size += sizeof(diagnostic_field.device_model); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_selector)); - total_size += sizeof(diagnostic_field.status_selector); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_flags)); - total_size += sizeof(diagnostic_field.status_flags); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_state)); - total_size += sizeof(diagnostic_field.system_state); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_timer_ms)); - total_size += sizeof(diagnostic_field.system_timer_ms); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_stream_enabled)); - total_size += sizeof(diagnostic_field.imu_stream_enabled); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_stream_enabled)); - total_size += sizeof(diagnostic_field.filter_stream_enabled); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_dropped_packets)); - total_size += sizeof(diagnostic_field.imu_dropped_packets); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_dropped_packets)); - total_size += sizeof(diagnostic_field.filter_dropped_packets); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_written)); - total_size += sizeof(diagnostic_field.com1_port_bytes_written); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_read)); - total_size += sizeof(diagnostic_field.com1_port_bytes_read); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_write_overruns)); - total_size += sizeof(diagnostic_field.com1_port_write_overruns); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_read_overruns)); - total_size += sizeof(diagnostic_field.com1_port_read_overruns); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_parser_errors)); - total_size += sizeof(diagnostic_field.imu_parser_errors); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_message_count)); - total_size += sizeof(diagnostic_field.imu_message_count); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_last_message_ms)); + if(response_size != sizeof(gx4_25_diagnostic_device_status_field)) + { + return MIP_INTERFACE_ERROR; } - void * struct_pointer; - struct_pointer = &diagnostic_field; - total_size = 0; - - memcpy(struct_pointer, response_buffer, sizeof(diagnostic_field.device_model)); - total_size += sizeof(diagnostic_field.device_model); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_selector)); - total_size += sizeof(diagnostic_field.status_selector); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_flags)); - total_size += sizeof(diagnostic_field.status_flags); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_state)); - total_size += sizeof(diagnostic_field.system_state); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_timer_ms)); - total_size += sizeof(diagnostic_field.system_timer_ms); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_stream_enabled)); - total_size += sizeof(diagnostic_field.imu_stream_enabled); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_stream_enabled)); - total_size += sizeof(diagnostic_field.filter_stream_enabled); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_dropped_packets)); - total_size += sizeof(diagnostic_field.imu_dropped_packets); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_dropped_packets)); - total_size += sizeof(diagnostic_field.filter_dropped_packets); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_written)); - total_size += sizeof(diagnostic_field.com1_port_bytes_written); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_read)); - total_size += sizeof(diagnostic_field.com1_port_bytes_read); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_write_overruns)); - total_size += sizeof(diagnostic_field.com1_port_write_overruns); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_read_overruns)); - total_size += sizeof(diagnostic_field.com1_port_read_overruns); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_parser_errors)); - total_size += sizeof(diagnostic_field.imu_parser_errors); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_message_count)); - total_size += sizeof(diagnostic_field.imu_message_count); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_last_message_ms)); - total_size += sizeof(diagnostic_field.imu_last_message_ms); + else if(MIP_SDK_CONFIG_BYTESWAP) + { - } - else - return MIP_INTERFACE_ERROR; + //byteswap and copy response to diagnostic status struct + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.device_model)); + total_size += sizeof(diagnostic_field.device_model); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_selector)); + total_size += sizeof(diagnostic_field.status_selector); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_flags)); + total_size += sizeof(diagnostic_field.status_flags); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_state)); + total_size += sizeof(diagnostic_field.system_state); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_timer_ms)); + total_size += sizeof(diagnostic_field.system_timer_ms); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_stream_enabled)); + total_size += sizeof(diagnostic_field.imu_stream_enabled); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_stream_enabled)); + total_size += sizeof(diagnostic_field.filter_stream_enabled); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_dropped_packets)); + total_size += sizeof(diagnostic_field.imu_dropped_packets); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_dropped_packets)); + total_size += sizeof(diagnostic_field.filter_dropped_packets); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_written)); + total_size += sizeof(diagnostic_field.com1_port_bytes_written); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_read)); + total_size += sizeof(diagnostic_field.com1_port_bytes_read); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_write_overruns)); + total_size += sizeof(diagnostic_field.com1_port_write_overruns); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_read_overruns)); + total_size += sizeof(diagnostic_field.com1_port_read_overruns); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_parser_errors)); + total_size += sizeof(diagnostic_field.imu_parser_errors); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_message_count)); + total_size += sizeof(diagnostic_field.imu_message_count); + byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_last_message_ms)); + } - return MIP_INTERFACE_OK; - } - } + void * struct_pointer; + struct_pointer = &diagnostic_field; + total_size = 0; + + memcpy(struct_pointer, response_buffer, sizeof(diagnostic_field.device_model)); + total_size += sizeof(diagnostic_field.device_model); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_selector)); + total_size += sizeof(diagnostic_field.status_selector); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_flags)); + total_size += sizeof(diagnostic_field.status_flags); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_state)); + total_size += sizeof(diagnostic_field.system_state); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_timer_ms)); + total_size += sizeof(diagnostic_field.system_timer_ms); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_stream_enabled)); + total_size += sizeof(diagnostic_field.imu_stream_enabled); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_stream_enabled)); + total_size += sizeof(diagnostic_field.filter_stream_enabled); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_dropped_packets)); + total_size += sizeof(diagnostic_field.imu_dropped_packets); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_dropped_packets)); + total_size += sizeof(diagnostic_field.filter_dropped_packets); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_written)); + total_size += sizeof(diagnostic_field.com1_port_bytes_written); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_read)); + total_size += sizeof(diagnostic_field.com1_port_bytes_read); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_write_overruns)); + total_size += sizeof(diagnostic_field.com1_port_write_overruns); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_read_overruns)); + total_size += sizeof(diagnostic_field.com1_port_read_overruns); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_parser_errors)); + total_size += sizeof(diagnostic_field.imu_parser_errors); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_message_count)); + total_size += sizeof(diagnostic_field.imu_message_count); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_last_message_ms)); + total_size += sizeof(diagnostic_field.imu_last_message_ms); + } + else + return MIP_INTERFACE_ERROR; + return MIP_INTERFACE_OK; + } + } //Start callbacks for data packets - //////////////////////////////////////////////////////////////////////////////// // // Filter Packet Callback @@ -2559,258 +2838,261 @@ namespace Microstrain // If we aren't publishing, then return if (!publish_odom_) return; + //ROS_INFO("Filter callback"); //The packet callback can have several types, process them all switch(callback_type) - { - /// - //Handle valid packets - /// - + { + /// + //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) - { - /// - // Estimated LLH Position - /// - - case MIP_FILTER_DATA_LLH_POS: - { - memcpy(&curr_filter_pos_, field_data, sizeof(mip_filter_llh_pos)); + { + filter_valid_packet_count_++; - //For little-endian targets, byteswap the data field - mip_filter_llh_pos_byteswap(&curr_filter_pos_); + /// + //Loop through all of the data fields + /// - nav_msg_.header.seq = filter_valid_packet_count_; - nav_msg_.header.stamp = ros::Time::now(); - nav_msg_.header.frame_id = odom_frame_id_; - nav_msg_.child_frame_id = odom_child_frame_id_; - nav_msg_.pose.pose.position.y = curr_filter_pos_.latitude; - nav_msg_.pose.pose.position.x = curr_filter_pos_.longitude; - nav_msg_.pose.pose.position.z = curr_filter_pos_.ellipsoid_height; + while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + { - }break; + /// + // Decode the field + /// - /// - // Estimated NED Velocity - /// + switch(field_header->descriptor) + { + /// + // Estimated LLH Position + /// - case MIP_FILTER_DATA_NED_VEL: - { - memcpy(&curr_filter_vel_, field_data, sizeof(mip_filter_ned_velocity)); + case MIP_FILTER_DATA_LLH_POS: + { + memcpy(&curr_filter_pos_, field_data, sizeof(mip_filter_llh_pos)); - //For little-endian targets, byteswap the data field - mip_filter_ned_velocity_byteswap(&curr_filter_vel_); + //For little-endian targets, byteswap the data field + mip_filter_llh_pos_byteswap(&curr_filter_pos_); - // rotate velocities from NED to sensor coordinates - // Constructor takes x, y, z , w - tf2::Quaternion nav_quat(curr_filter_quaternion_.q[2], - curr_filter_quaternion_.q[1], - -1.0*curr_filter_quaternion_.q[3], - curr_filter_quaternion_.q[0]); + nav_msg_.header.seq = filter_valid_packet_count_; + nav_msg_.header.stamp = ros::Time::now(); + nav_msg_.header.frame_id = odom_frame_id_; + nav_msg_.child_frame_id = odom_child_frame_id_; + nav_msg_.pose.pose.position.y = curr_filter_pos_.latitude; + nav_msg_.pose.pose.position.x = curr_filter_pos_.longitude; + nav_msg_.pose.pose.position.z = curr_filter_pos_.ellipsoid_height; - tf2::Vector3 vel_enu(curr_filter_vel_.east, - curr_filter_vel_.north, - -1.0*curr_filter_vel_.down); - tf2::Vector3 vel_in_sensor_frame = tf2::quatRotate(nav_quat.inverse(),vel_enu); + }break; - nav_msg_.twist.twist.linear.x = vel_in_sensor_frame[0]; //curr_filter_vel_.east; - nav_msg_.twist.twist.linear.y = vel_in_sensor_frame[1]; //curr_filter_vel_.north; - nav_msg_.twist.twist.linear.z = vel_in_sensor_frame[2]; //-1*curr_filter_vel_.down; - }break; - - /// - // Estimated Attitude, Euler Angles - /// + /// + // Estimated NED Velocity + /// - case MIP_FILTER_DATA_ATT_EULER_ANGLES: - { - memcpy(&curr_filter_angles_, field_data, sizeof(mip_filter_attitude_euler_angles)); + case MIP_FILTER_DATA_NED_VEL: + { + memcpy(&curr_filter_vel_, field_data, sizeof(mip_filter_ned_velocity)); + + //For little-endian targets, byteswap the data field + mip_filter_ned_velocity_byteswap(&curr_filter_vel_); - //For little-endian targets, byteswap the data field - mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_); + // rotate velocities from NED to sensor coordinates + // Constructor takes x, y, z , w + tf2::Quaternion nav_quat(curr_filter_quaternion_.q[2], + curr_filter_quaternion_.q[1], + -1.0*curr_filter_quaternion_.q[3], + curr_filter_quaternion_.q[0]); - }break; + tf2::Vector3 vel_enu(curr_filter_vel_.east, + curr_filter_vel_.north, + -1.0*curr_filter_vel_.down); + tf2::Vector3 vel_in_sensor_frame = tf2::quatRotate(nav_quat.inverse(),vel_enu); - // Quaternion - case MIP_FILTER_DATA_ATT_QUATERNION: - { - memcpy(&curr_filter_quaternion_, field_data, sizeof(mip_filter_attitude_quaternion)); + nav_msg_.twist.twist.linear.x = vel_in_sensor_frame[0]; //curr_filter_vel_.east; + nav_msg_.twist.twist.linear.y = vel_in_sensor_frame[1]; //curr_filter_vel_.north; + nav_msg_.twist.twist.linear.z = vel_in_sensor_frame[2]; //-1*curr_filter_vel_.down; + }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_quaternion_byteswap(&curr_filter_quaternion_); + //For little-endian targets, byteswap the data field + mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_); + }break; - // put into ENU - swap X/Y, invert Z - nav_msg_.pose.pose.orientation.x = curr_filter_quaternion_.q[2]; - nav_msg_.pose.pose.orientation.y = curr_filter_quaternion_.q[1]; - nav_msg_.pose.pose.orientation.z = -1.0*curr_filter_quaternion_.q[3]; - nav_msg_.pose.pose.orientation.w = curr_filter_quaternion_.q[0]; + // Quaternion + case MIP_FILTER_DATA_ATT_QUATERNION: + { + memcpy(&curr_filter_quaternion_, field_data, sizeof(mip_filter_attitude_quaternion)); - }break; + //For little-endian targets, byteswap the data field + mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); - // Angular Rates - case MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE: - { - memcpy(&curr_filter_angular_rate_, field_data, sizeof(mip_filter_compensated_angular_rate)); + // put into ENU - swap X/Y, invert Z + nav_msg_.pose.pose.orientation.x = curr_filter_quaternion_.q[2]; + nav_msg_.pose.pose.orientation.y = curr_filter_quaternion_.q[1]; + nav_msg_.pose.pose.orientation.z = -1.0*curr_filter_quaternion_.q[3]; + nav_msg_.pose.pose.orientation.w = curr_filter_quaternion_.q[0]; - //For little-endian targets, byteswap the data field - mip_filter_compensated_angular_rate_byteswap(&curr_filter_angular_rate_); + }break; - nav_msg_.twist.twist.angular.x = curr_filter_angular_rate_.x; - nav_msg_.twist.twist.angular.y = curr_filter_angular_rate_.y; - nav_msg_.twist.twist.angular.z = curr_filter_angular_rate_.z; + // 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_); - }break; + nav_msg_.twist.twist.angular.x = curr_filter_angular_rate_.x; + nav_msg_.twist.twist.angular.y = curr_filter_angular_rate_.y; + nav_msg_.twist.twist.angular.z = curr_filter_angular_rate_.z; - // Position Uncertainty - case MIP_FILTER_DATA_POS_UNCERTAINTY: - { - memcpy(&curr_filter_pos_uncertainty_, field_data, sizeof(mip_filter_llh_pos_uncertainty)); - //For little-endian targets, byteswap the data field - mip_filter_llh_pos_uncertainty_byteswap(&curr_filter_pos_uncertainty_); + }break; - //x-axis - nav_msg_.pose.covariance[0] = curr_filter_pos_uncertainty_.east*curr_filter_pos_uncertainty_.east; - nav_msg_.pose.covariance[7] = curr_filter_pos_uncertainty_.north*curr_filter_pos_uncertainty_.north; - nav_msg_.pose.covariance[14] = curr_filter_pos_uncertainty_.down*curr_filter_pos_uncertainty_.down; - }break; + // Position Uncertainty + case MIP_FILTER_DATA_POS_UNCERTAINTY: + { + memcpy(&curr_filter_pos_uncertainty_, field_data, sizeof(mip_filter_llh_pos_uncertainty)); - // Velocity Uncertainty - case MIP_FILTER_DATA_VEL_UNCERTAINTY: - { - memcpy(&curr_filter_vel_uncertainty_, field_data, sizeof(mip_filter_ned_vel_uncertainty)); + //For little-endian targets, byteswap the data field + mip_filter_llh_pos_uncertainty_byteswap(&curr_filter_pos_uncertainty_); + + //x-axis + nav_msg_.pose.covariance[0] = curr_filter_pos_uncertainty_.east*curr_filter_pos_uncertainty_.east; + nav_msg_.pose.covariance[7] = curr_filter_pos_uncertainty_.north*curr_filter_pos_uncertainty_.north; + nav_msg_.pose.covariance[14] = curr_filter_pos_uncertainty_.down*curr_filter_pos_uncertainty_.down; + }break; - //For little-endian targets, byteswap the data field - mip_filter_ned_vel_uncertainty_byteswap(&curr_filter_vel_uncertainty_); + // Velocity Uncertainty + case MIP_FILTER_DATA_VEL_UNCERTAINTY: + { + memcpy(&curr_filter_vel_uncertainty_, field_data, sizeof(mip_filter_ned_vel_uncertainty)); - nav_msg_.twist.covariance[0] = curr_filter_vel_uncertainty_.east*curr_filter_vel_uncertainty_.east; - nav_msg_.twist.covariance[7] = curr_filter_vel_uncertainty_.north*curr_filter_vel_uncertainty_.north; - nav_msg_.twist.covariance[14] = curr_filter_vel_uncertainty_.down*curr_filter_vel_uncertainty_.down; + //For little-endian targets, byteswap the data field + mip_filter_ned_vel_uncertainty_byteswap(&curr_filter_vel_uncertainty_); - }break; + nav_msg_.twist.covariance[0] = curr_filter_vel_uncertainty_.east*curr_filter_vel_uncertainty_.east; + nav_msg_.twist.covariance[7] = curr_filter_vel_uncertainty_.north*curr_filter_vel_uncertainty_.north; + nav_msg_.twist.covariance[14] = curr_filter_vel_uncertainty_.down*curr_filter_vel_uncertainty_.down; - // Attitude Uncertainty - case MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER: - { - memcpy(&curr_filter_att_uncertainty_, field_data, sizeof(mip_filter_euler_attitude_uncertainty)); + }break; - //For little-endian targets, byteswap the data field - mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_); - nav_msg_.pose.covariance[21] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; - nav_msg_.pose.covariance[28] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; - nav_msg_.pose.covariance[35] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; + // Attitude Uncertainty + case MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER: + { + memcpy(&curr_filter_att_uncertainty_, field_data, sizeof(mip_filter_euler_attitude_uncertainty)); - }break; + //For little-endian targets, byteswap the data field + mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_); + nav_msg_.pose.covariance[21] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; + nav_msg_.pose.covariance[28] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; + nav_msg_.pose.covariance[35] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; - // Filter Status - case MIP_FILTER_DATA_FILTER_STATUS: - { - memcpy(&curr_filter_status_, field_data, sizeof(mip_filter_status)); + }break; - //For little-endian targets, byteswap the data field - mip_filter_status_byteswap(&curr_filter_status_); + // Filter Status + case MIP_FILTER_DATA_FILTER_STATUS: + { + memcpy(&curr_filter_status_, field_data, sizeof(mip_filter_status)); - nav_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); - nav_status_msg_.data.push_back(curr_filter_status_.filter_state); - nav_status_msg_.data.push_back(curr_filter_status_.dynamics_mode); - nav_status_msg_.data.push_back(curr_filter_status_.status_flags); - nav_status_pub_.publish(nav_status_msg_); + //For little-endian targets, byteswap the data field + mip_filter_status_byteswap(&curr_filter_status_); + nav_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); + nav_status_msg_.data.push_back(curr_filter_status_.filter_state); + nav_status_msg_.data.push_back(curr_filter_status_.dynamics_mode); + nav_status_msg_.data.push_back(curr_filter_status_.status_flags); + nav_status_pub_.publish(nav_status_msg_); - }break; - default: break; - } - } + }break; - // Publish - nav_pub_.publish(nav_msg_); - }break; + default: break; + } + } + // Publish + nav_pub_.publish(nav_msg_); + }break; - /// - //Handle checksum error packets - /// + /// + //Handle checksum error packets + /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: - { - filter_checksum_error_packet_count_++; - }break; + { + filter_checksum_error_packet_count_++; + }break; - /// - //Handle timeout packets - /// + /// + //Handle timeout packets + /// case MIP_INTERFACE_CALLBACK_TIMEOUT: - { - filter_timeout_packet_count_++; - }break; + { + filter_timeout_packet_count_++; + }break; + default: break; - } + } print_packet_stats(); } // filter_packet_callback -//Send diagnostic information to device status topic and diagnostic aggregator -void Microstrain::device_status_callback() -{ - if(GX5_25){ - u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; - start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK){ - if (clock() - start > 5000){ - ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); - break; + //Send diagnostic information to device status topic and diagnostic aggregator + void Microstrain::device_status_callback() + { + if(GX5_25) + { + u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; + start = clock(); + while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + { + if(clock() - start > 5000) + { + ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); + break; + } } - } - //ROS_INFO("Adding device diagnostics to status msg"); - //ROS_INFO("adding data to message"); - device_status_msg_.device_model = diagnostic_field.device_model; - device_status_msg_.status_selector = diagnostic_field.status_selector; - device_status_msg_.status_flags = diagnostic_field.status_flags; - device_status_msg_.system_state = diagnostic_field.system_state; - device_status_msg_.system_timer_ms = diagnostic_field.system_timer_ms; - device_status_msg_.imu_stream_enabled = diagnostic_field.imu_stream_enabled; - device_status_msg_.filter_stream_enabled = diagnostic_field.filter_stream_enabled; - device_status_msg_.imu_dropped_packets = diagnostic_field.imu_dropped_packets; - device_status_msg_.filter_dropped_packets = diagnostic_field.filter_dropped_packets; - device_status_msg_.com1_port_bytes_written = diagnostic_field.com1_port_bytes_written; - device_status_msg_.com1_port_bytes_read = diagnostic_field.com1_port_bytes_read; - device_status_msg_.com1_port_write_overruns = diagnostic_field.com1_port_write_overruns; - device_status_msg_.com1_port_read_overruns = diagnostic_field.com1_port_read_overruns; - device_status_msg_.imu_parser_errors = diagnostic_field.imu_parser_errors; - device_status_msg_.imu_message_count = diagnostic_field.imu_message_count; - device_status_msg_.imu_last_message_ms = diagnostic_field.imu_last_message_ms; - - device_status_pub_.publish(device_status_msg_); - } - else{ - ROS_INFO("Device status messages not configured for this model"); + //ROS_INFO("Adding device diagnostics to status msg"); + //ROS_INFO("adding data to message"); + device_status_msg_.device_model = diagnostic_field.device_model; + device_status_msg_.status_selector = diagnostic_field.status_selector; + device_status_msg_.status_flags = diagnostic_field.status_flags; + device_status_msg_.system_state = diagnostic_field.system_state; + device_status_msg_.system_timer_ms = diagnostic_field.system_timer_ms; + device_status_msg_.imu_stream_enabled = diagnostic_field.imu_stream_enabled; + device_status_msg_.filter_stream_enabled = diagnostic_field.filter_stream_enabled; + device_status_msg_.imu_dropped_packets = diagnostic_field.imu_dropped_packets; + device_status_msg_.filter_dropped_packets = diagnostic_field.filter_dropped_packets; + device_status_msg_.com1_port_bytes_written = diagnostic_field.com1_port_bytes_written; + device_status_msg_.com1_port_bytes_read = diagnostic_field.com1_port_bytes_read; + device_status_msg_.com1_port_write_overruns = diagnostic_field.com1_port_write_overruns; + device_status_msg_.com1_port_read_overruns = diagnostic_field.com1_port_read_overruns; + device_status_msg_.imu_parser_errors = diagnostic_field.imu_parser_errors; + device_status_msg_.imu_message_count = diagnostic_field.imu_message_count; + device_status_msg_.imu_last_message_ms = diagnostic_field.imu_last_message_ms; + + device_status_pub_.publish(device_status_msg_); + } + else + { + ROS_INFO("Device status messages not configured for this model"); + } } -} //////////////////////////////////////////////////////////////////////////////// // @@ -2828,132 +3110,132 @@ void Microstrain::device_status_callback() return; //The packet callback can have several types, process them all switch(callback_type) - { - /// - //Handle valid packets - /// + { + /// + //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 - imu_msg_.header.seq = ahrs_valid_packet_count_; - imu_msg_.header.stamp = ros::Time::now(); - imu_msg_.header.frame_id = imu_frame_id_; - imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0]; - imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1]; - imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2]; - // Since the sensor does not produce a covariance for linear acceleration, set it based - // on our pulled in parameters. - std::copy( imu_linear_cov_.begin(), imu_linear_cov_.end(), imu_msg_.linear_acceleration_covariance.begin()); - - }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_); - - imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0]; - imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[1]; - imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2]; - // Since the sensor does not produce a covariance for angular velocity, set it based - // on our pulled in parameters. - std::copy( imu_angular_cov_.begin(), imu_angular_cov_.end(), imu_msg_.angular_velocity_covariance.begin()); - - }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_); - - }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_); - // put into ENU - swap X/Y, invert Z - imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; - imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; - imu_msg_.orientation.z = -1.0*curr_ahrs_quaternion_.q[3]; - imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; - // Since the MIP_AHRS data does not contain uncertainty values we have to set them based on the parameter values. - std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(), imu_msg_.orientation_covariance.begin()); - - }break; - - default: break; - } - } - - // Publish - imu_pub_.publish(imu_msg_); - - }break; - - /// - //Handle checksum error packets - /// + { + 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 + imu_msg_.header.seq = ahrs_valid_packet_count_; + imu_msg_.header.stamp = ros::Time::now(); + imu_msg_.header.frame_id = imu_frame_id_; + imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0]; + imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1]; + imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2]; + // Since the sensor does not produce a covariance for linear acceleration, set it based + // on our pulled in parameters. + std::copy( imu_linear_cov_.begin(), imu_linear_cov_.end(), imu_msg_.linear_acceleration_covariance.begin()); + + }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_); + + imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0]; + imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[1]; + imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2]; + // Since the sensor does not produce a covariance for angular velocity, set it based + // on our pulled in parameters. + std::copy( imu_angular_cov_.begin(), imu_angular_cov_.end(), imu_msg_.angular_velocity_covariance.begin()); + + }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_); + + }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_); + // put into ENU - swap X/Y, invert Z + imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; + imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; + imu_msg_.orientation.z = -1.0*curr_ahrs_quaternion_.q[3]; + imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; + // Since the MIP_AHRS data does not contain uncertainty values we have to set them based on the parameter values. + std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(), imu_msg_.orientation_covariance.begin()); + + }break; + + default: break; + } + } + + // Publish + imu_pub_.publish(imu_msg_); + + }break; + + /// + //Handle checksum error packets + /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: - { - ahrs_checksum_error_packet_count_++; - }break; + { + ahrs_checksum_error_packet_count_++; + }break; - /// - //Handle timeout packets - /// + /// + //Handle timeout packets + /// case MIP_INTERFACE_CALLBACK_TIMEOUT: - { - ahrs_timeout_packet_count_++; - }break; - default: break; - } + { + ahrs_timeout_packet_count_++; + }break; + default: break; + } print_packet_stats(); } // ahrs_packet_callback @@ -2975,113 +3257,114 @@ void Microstrain::device_status_callback() return; //The packet callback can have several types, process them all switch(callback_type) - { - /// - //Handle valid packets - /// + { + /// + //Handle valid packets + /// case MIP_INTERFACE_CALLBACK_VALID_PACKET: - { - gps_valid_packet_count_++; - - /// - //Loop through all of the data fields - /// + { + gps_valid_packet_count_++; - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) - { + /// + //Loop through all of the data fields + /// - /// - // Decode the field - /// + while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + { - switch(field_header->descriptor) - { - /// - // LLH Position - /// + /// + // Decode the field + /// - case MIP_GPS_DATA_LLH_POS: - { - memcpy(&curr_llh_pos_, field_data, sizeof(mip_gps_llh_pos)); + switch(field_header->descriptor) + { + /// + // LLH Position + /// - //For little-endian targets, byteswap the data field - mip_gps_llh_pos_byteswap(&curr_llh_pos_); + case MIP_GPS_DATA_LLH_POS: + { + memcpy(&curr_llh_pos_, field_data, sizeof(mip_gps_llh_pos)); - // push into ROS message - gps_msg_.latitude = curr_llh_pos_.latitude; - gps_msg_.longitude = curr_llh_pos_.longitude; - gps_msg_.altitude = curr_llh_pos_.ellipsoid_height; - gps_msg_.position_covariance_type = 2; // diagnals known - gps_msg_.position_covariance[0] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy; - gps_msg_.position_covariance[4] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy; - gps_msg_.position_covariance[8] = curr_llh_pos_.vertical_accuracy*curr_llh_pos_.vertical_accuracy; - gps_msg_.status.status = curr_llh_pos_.valid_flags - 1; - gps_msg_.status.service = 1; // assumed - // Header - gps_msg_.header.seq = gps_valid_packet_count_; - gps_msg_.header.stamp = ros::Time::now(); - gps_msg_.header.frame_id = gps_frame_id_; + //For little-endian targets, byteswap the data field + mip_gps_llh_pos_byteswap(&curr_llh_pos_); - }break; + // push into ROS message + gps_msg_.latitude = curr_llh_pos_.latitude; + gps_msg_.longitude = curr_llh_pos_.longitude; + gps_msg_.altitude = curr_llh_pos_.ellipsoid_height; + gps_msg_.position_covariance_type = 2; // diagnals known + gps_msg_.position_covariance[0] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy; + gps_msg_.position_covariance[4] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy; + gps_msg_.position_covariance[8] = curr_llh_pos_.vertical_accuracy*curr_llh_pos_.vertical_accuracy; + gps_msg_.status.status = curr_llh_pos_.valid_flags - 1; + gps_msg_.status.service = 1; // assumed + // Header + gps_msg_.header.seq = gps_valid_packet_count_; + gps_msg_.header.stamp = ros::Time::now(); + gps_msg_.header.frame_id = gps_frame_id_; - /// - // NED Velocity - /// + }break; - case MIP_GPS_DATA_NED_VELOCITY: - { - memcpy(&curr_ned_vel_, field_data, sizeof(mip_gps_ned_vel)); + /// + // NED Velocity + /// - //For little-endian targets, byteswap the data field - mip_gps_ned_vel_byteswap(&curr_ned_vel_); + case MIP_GPS_DATA_NED_VELOCITY: + { + memcpy(&curr_ned_vel_, field_data, sizeof(mip_gps_ned_vel)); - }break; + //For little-endian targets, byteswap the data field + mip_gps_ned_vel_byteswap(&curr_ned_vel_); - /// - // GPS Time - /// + }break; - case MIP_GPS_DATA_GPS_TIME: - { - memcpy(&curr_gps_time_, field_data, sizeof(mip_gps_time)); + /// + // GPS Time + /// - //For little-endian targets, byteswap the data field - mip_gps_time_byteswap(&curr_gps_time_); + case MIP_GPS_DATA_GPS_TIME: + { + memcpy(&curr_gps_time_, field_data, sizeof(mip_gps_time)); - }break; + //For little-endian targets, byteswap the data field + mip_gps_time_byteswap(&curr_gps_time_); - default: break; - } - } - }break; + }break; + default: break; + } + } + }break; - /// - //Handle checksum error packets - /// + /// + //Handle checksum error packets + /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: - { - msgvalid = 0; - gps_checksum_error_packet_count_++; - }break; + { + msgvalid = 0; + gps_checksum_error_packet_count_++; + }break; - /// - //Handle timeout packets - /// + /// + //Handle timeout packets + /// case MIP_INTERFACE_CALLBACK_TIMEOUT: - { - msgvalid = 0; - gps_timeout_packet_count_++; - }break; + { + msgvalid = 0; + gps_timeout_packet_count_++; + }break; + default: break; - } + } - if (msgvalid){ + if (msgvalid) + { // Publish the message - gps_pub_.publish(gps_msg_); + gps_pub_.publish(gps_msg_); } print_packet_stats(); @@ -3090,8 +3373,8 @@ void Microstrain::device_status_callback() void Microstrain::print_packet_stats() { ROS_DEBUG_THROTTLE(1.0,"%u FILTER (%u errors) %u AHRS (%u errors) %u GPS (%u errors) Packets", 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_, - gps_valid_packet_count_, gps_timeout_packet_count_ + gps_checksum_error_packet_count_); + ahrs_valid_packet_count_, ahrs_timeout_packet_count_ + ahrs_checksum_error_packet_count_, + gps_valid_packet_count_, gps_timeout_packet_count_ + gps_checksum_error_packet_count_); } // print_packet_stats From 43ae3850ca9571bd5e7a52de54dbd451ad78a71a Mon Sep 17 00:00:00 2001 From: Sam Date: Wed, 10 Jul 2019 13:49:04 -0400 Subject: [PATCH 4/4] Add roslint and cleaned up files accordingly. --- CMakeLists.txt | 5 + package.xml | 1 + src/microstrain_3dm.cpp | 1851 +++++++++++++++++++++++---------------- 3 files changed, 1094 insertions(+), 763 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c990ef4..5e949d4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ project(microstrain_mips) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + roslint message_generation geometry_msgs nav_msgs @@ -479,3 +480,7 @@ install(DIRECTORY launch config ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +## Run roslint on only the node files for now +# TODO: Add more files as needed +roslint_cpp(src/microstrain_3dm.cpp src/microstrain_3dm_node.cpp) diff --git a/package.xml b/package.xml index 8a40a132..e3ac76cc 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ nav_msgs message_generation + roslint message_runtime diagnostic_updater diagnostic_aggregator diff --git a/src/microstrain_3dm.cpp b/src/microstrain_3dm.cpp index 37c7226e..4a81322c 100644 --- a/src/microstrain_3dm.cpp +++ b/src/microstrain_3dm.cpp @@ -27,6 +27,7 @@ along with Foobar. If not, see . #include "microstrain_mips/status_msg.h" #include "microstrain_diagnostic_updater.h" +#include namespace Microstrain { @@ -48,9 +49,9 @@ namespace Microstrain publish_gps_(true), publish_imu_(true), publish_odom_(true), - imu_linear_cov_(std::vector(9,0.0)), - imu_angular_cov_(std::vector(9,0.0)), - imu_orientation_cov_(std::vector(9,0.0)) + imu_linear_cov_(std::vector(9, 0.0)), + imu_angular_cov_(std::vector(9, 0.0)), + imu_orientation_cov_(std::vector(9, 0.0)) { // pass } @@ -96,7 +97,7 @@ namespace Microstrain com_mode = 0; - //Device model flags + // Device model flags GX5_15 = false; GX5_25 = false; GX5_35 = false; @@ -112,112 +113,159 @@ namespace Microstrain std::string port; int baud, pdyn_mode; private_nh.param("port", port, std::string("/dev/ttyACM1")); - private_nh.param("baudrate",baud,115200); + 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); - private_nh.param("save_settings",save_settings,true); + private_nh.param("device_setup", device_setup, false); + private_nh.param("readback_settings", readback_settings, true); + private_nh.param("save_settings", save_settings, true); - private_nh.param("auto_init",auto_init,true); - private_nh.param("gps_rate",gps_rate_, 1); - private_nh.param("imu_rate",imu_rate_, 10); - private_nh.param("nav_rate",nav_rate_, 10); - private_nh.param("dynamics_mode",pdyn_mode,1); + private_nh.param("auto_init", auto_init, true); + private_nh.param("gps_rate", gps_rate_, 1); + private_nh.param("imu_rate", imu_rate_, 10); + private_nh.param("nav_rate", nav_rate_, 10); + private_nh.param("dynamics_mode", pdyn_mode, 1); dynamics_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); + ROS_WARN("dynamics_mode can't be %#04X, must be 1, 2 or 3. Setting to 1.", dynamics_mode); dynamics_mode = 1; } - private_nh.param("declination_source",declination_source,2); + private_nh.param("declination_source", declination_source, 2); 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); + 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; - //declination_source_command=(u8)declination_source; - private_nh.param("declination",declination,0.23); - private_nh.param("gps_frame_id",gps_frame_id_, std::string("wgs84")); - private_nh.param("imu_frame_id",imu_frame_id_, std::string("base_link")); - private_nh.param("odom_frame_id",odom_frame_id_, std::string("wgs84")); - private_nh.param("odom_child_frame_id",odom_child_frame_id_, - std::string("base_link")); + // declination_source_command=(u8)declination_source; + private_nh.param("declination", declination, 0.23); + private_nh.param("gps_frame_id", gps_frame_id_, std::string("wgs84")); + private_nh.param("imu_frame_id", imu_frame_id_, std::string("base_link")); + private_nh.param("odom_frame_id", odom_frame_id_, std::string("wgs84")); + private_nh.param("odom_child_frame_id", odom_child_frame_id_, std::string("base_link")); - private_nh.param("publish_imu",publish_imu_, true); - private_nh.param("publish_bias",publish_bias_, true); + private_nh.param("publish_imu", publish_imu_, true); + private_nh.param("publish_bias", publish_bias_, true); // Covariance parameters to set the sensor_msg/IMU covariance values - std::vector default_cov(9,0.0); + std::vector default_cov(9, 0.0); private_nh.param("imu_orientation_cov", imu_orientation_cov_, default_cov); - private_nh.param("imu_linear_cov",imu_linear_cov_, default_cov); - private_nh.param("imu_angular_cov",imu_angular_cov_, default_cov); + private_nh.param("imu_linear_cov", imu_linear_cov_, default_cov); + private_nh.param("imu_angular_cov", imu_angular_cov_, default_cov); // ROS publishers and subscribers if (publish_imu_) - imu_pub_ = node.advertise("imu/data",100); + imu_pub_ = node.advertise("imu/data", 100); - //Publishes device status + // Publishes device status device_status_pub_ = node.advertise("device/status", 100); - //Services to set/get device functions - ros::ServiceServer reset_filter = node.advertiseService("reset_kf", &Microstrain::reset_callback, this); - ros::ServiceServer device_report_service = node.advertiseService("device_report", &Microstrain::device_report, this); - ros::ServiceServer gyro_bias_capture_service = node.advertiseService("gyro_bias_capture", &Microstrain::gyro_bias_capture, this); - ros::ServiceServer set_soft_iron_matrix_service = node.advertiseService("set_soft_iron_matrix", &Microstrain::set_soft_iron_matrix, this); - ros::ServiceServer set_complementary_filter_service = node.advertiseService("set_complementary_filter", &Microstrain::set_complementary_filter, this); - ros::ServiceServer set_filter_euler_service = node.advertiseService("set_filter_euler", &Microstrain::set_filter_euler, this); - ros::ServiceServer set_filter_heading_service = node.advertiseService("set_filter_heading", &Microstrain::set_filter_heading, this); - ros::ServiceServer set_accel_bias_model_service = node.advertiseService("set_accel_bias_model", &Microstrain::set_accel_bias_model, this); - ros::ServiceServer set_accel_adaptive_vals_service = node.advertiseService("set_accel_adaptive_vals", &Microstrain::set_accel_adaptive_vals, this); - ros::ServiceServer set_sensor_vehicle_frame_trans_service = node.advertiseService("set_sensor_vehicle_frame_trans", &Microstrain::set_sensor_vehicle_frame_trans, this); - ros::ServiceServer set_sensor_vehicle_frame_offset_service = node.advertiseService("set_sensor_vehicle_frame_offset", &Microstrain::set_sensor_vehicle_frame_offset, this); - ros::ServiceServer set_accel_bias_service = node.advertiseService("set_accel_bias", &Microstrain::set_accel_bias, this); - ros::ServiceServer set_gyro_bias_service = node.advertiseService("set_gyro_bias", &Microstrain::set_gyro_bias, this); - ros::ServiceServer set_hard_iron_values_service = node.advertiseService("set_hard_iron_values", &Microstrain::set_hard_iron_values, this); - ros::ServiceServer get_accel_bias_service = node.advertiseService("get_accel_bias", &Microstrain::get_accel_bias, this); - ros::ServiceServer get_gyro_bias_service = node.advertiseService("get_gyro_bias", &Microstrain::get_gyro_bias, this); - ros::ServiceServer get_hard_iron_values_service = node.advertiseService("get_hard_iron_values", &Microstrain::get_hard_iron_values, this); - ros::ServiceServer get_soft_iron_matrix_service = node.advertiseService("get_soft_iron_matrix", &Microstrain::get_soft_iron_matrix, this); - ros::ServiceServer get_sensor_vehicle_frame_trans_service = node.advertiseService("get_sensor_vehicle_frame_trans", &Microstrain::get_sensor_vehicle_frame_trans, this); - ros::ServiceServer get_complementary_filter_service = node.advertiseService("get_complementary_filter", &Microstrain::get_complementary_filter, this); - ros::ServiceServer set_reference_position_service = node.advertiseService("set_reference_position", &Microstrain::set_reference_position, this); - ros::ServiceServer get_reference_position_service = node.advertiseService("get_reference_position", &Microstrain::get_reference_position, this); - ros::ServiceServer set_coning_sculling_comp_service = node.advertiseService("set_coning_sculling_comp", &Microstrain::set_coning_sculling_comp, this); - ros::ServiceServer get_coning_sculling_comp_service = node.advertiseService("get_coning_sculling_comp", &Microstrain::get_coning_sculling_comp, this); - ros::ServiceServer set_estimation_control_flags_service = node.advertiseService("set_estimation_control_flags", &Microstrain::set_estimation_control_flags, this); - ros::ServiceServer get_estimation_control_flags_service = node.advertiseService("get_estimation_control_flags", &Microstrain::get_estimation_control_flags, this); - ros::ServiceServer set_dynamics_mode_service = node.advertiseService("set_dynamics_mode", &Microstrain::set_dynamics_mode, this); - ros::ServiceServer get_basic_status_service = node.advertiseService("get_basic_status", &Microstrain::get_basic_status, this); - ros::ServiceServer get_diagnostic_report_service = node.advertiseService("get_diagnostic_report", &Microstrain::get_diagnostic_report, this); - ros::ServiceServer set_zero_angle_update_threshold_service = node.advertiseService("set_zero_angle_update_threshold", &Microstrain::set_zero_angle_update_threshold, this); - ros::ServiceServer get_zero_angle_update_threshold_service = node.advertiseService("get_zero_angle_update_threshold", &Microstrain::get_zero_angle_update_threshold, this); - ros::ServiceServer set_tare_orientation_service = node.advertiseService("set_tare_orientation", &Microstrain::set_tare_orientation, this); - ros::ServiceServer set_accel_noise_service = node.advertiseService("set_accel_noise", &Microstrain::set_accel_noise, this); - ros::ServiceServer get_accel_noise_service = node.advertiseService("get_accel_noise", &Microstrain::get_accel_noise, this); - ros::ServiceServer set_gyro_noise_service = node.advertiseService("set_gyro_noise", &Microstrain::set_gyro_noise, this); - ros::ServiceServer get_gyro_noise_service = node.advertiseService("get_gyro_noise", &Microstrain::get_gyro_noise, this); - ros::ServiceServer set_mag_noise_service = node.advertiseService("set_mag_noise", &Microstrain::set_mag_noise, this); - ros::ServiceServer get_mag_noise_service = node.advertiseService("get_mag_noise", &Microstrain::get_mag_noise, this); - ros::ServiceServer set_gyro_bias_model_service = node.advertiseService("set_gyro_bias_model", &Microstrain::set_gyro_bias_model, this); - ros::ServiceServer get_gyro_bias_model_service = node.advertiseService("get_gyro_bias_model", &Microstrain::get_gyro_bias_model, this); - ros::ServiceServer get_accel_adaptive_vals_service = node.advertiseService("get_accel_adaptive_vals", &Microstrain::get_accel_adaptive_vals, this); - ros::ServiceServer set_mag_adaptive_vals_service = node.advertiseService("set_mag_adaptive_vals", &Microstrain::set_mag_adaptive_vals, this); - ros::ServiceServer get_mag_adaptive_vals_service = node.advertiseService("get_mag_adaptive_vals", &Microstrain::get_mag_adaptive_vals, this); - ros::ServiceServer set_mag_dip_adaptive_vals_service = node.advertiseService("set_mag_dip_adaptive_vals", &Microstrain::set_mag_dip_adaptive_vals, this); - ros::ServiceServer get_accel_bias_model_service = node.advertiseService("get_accel_bias_model", &Microstrain::get_accel_bias_model, this); - ros::ServiceServer get_mag_dip_adaptive_vals_service = node.advertiseService("get_mag_dip_adaptive_vals", &Microstrain::get_mag_dip_adaptive_vals, this); - ros::ServiceServer get_sensor_vehicle_frame_offset_service = node.advertiseService("get_sensor_vehicle_frame_offset", &Microstrain::get_sensor_vehicle_frame_offset, this); - ros::ServiceServer get_dynamics_mode_service = node.advertiseService("get_dynamics_mode", &Microstrain::get_dynamics_mode, 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) + // Services to set/get device functions + ros::ServiceServer reset_filter = node.advertiseService("reset_kf", + &Microstrain::reset_callback, this); + ros::ServiceServer device_report_service = node.advertiseService("device_report", + &Microstrain::device_report, this); + ros::ServiceServer gyro_bias_capture_service = node.advertiseService("gyro_bias_capture", + &Microstrain::gyro_bias_capture, this); + ros::ServiceServer set_soft_iron_matrix_service = node.advertiseService("set_soft_iron_matrix", + &Microstrain::set_soft_iron_matrix, this); + ros::ServiceServer set_complementary_filter_service = node.advertiseService("set_complementary_filter", + &Microstrain::set_complementary_filter, this); + ros::ServiceServer set_filter_euler_service = node.advertiseService("set_filter_euler", + &Microstrain::set_filter_euler, this); + ros::ServiceServer set_filter_heading_service = node.advertiseService("set_filter_heading", + &Microstrain::set_filter_heading, this); + ros::ServiceServer set_accel_bias_model_service = node.advertiseService("set_accel_bias_model", + &Microstrain::set_accel_bias_model, this); + ros::ServiceServer set_accel_adaptive_vals_service = node.advertiseService("set_accel_adaptive_vals", + &Microstrain::set_accel_adaptive_vals, this); + ros::ServiceServer set_sensor_vehicle_frame_trans_service = node.advertiseService("set_sensor_vehicle_frame_trans", + &Microstrain::set_sensor_vehicle_frame_trans, this); + ros::ServiceServer set_sensor_vehicle_frame_offset_service = node.advertiseService("set_sensor_vehicle_frame_offset", + &Microstrain::set_sensor_vehicle_frame_offset, this); + ros::ServiceServer set_accel_bias_service = node.advertiseService("set_accel_bias", + &Microstrain::set_accel_bias, this); + ros::ServiceServer set_gyro_bias_service = node.advertiseService("set_gyro_bias", + &Microstrain::set_gyro_bias, this); + ros::ServiceServer set_hard_iron_values_service = node.advertiseService("set_hard_iron_values", + &Microstrain::set_hard_iron_values, this); + ros::ServiceServer get_accel_bias_service = node.advertiseService("get_accel_bias", + &Microstrain::get_accel_bias, this); + ros::ServiceServer get_gyro_bias_service = node.advertiseService("get_gyro_bias", + &Microstrain::get_gyro_bias, this); + ros::ServiceServer get_hard_iron_values_service = node.advertiseService("get_hard_iron_values", + &Microstrain::get_hard_iron_values, this); + ros::ServiceServer get_soft_iron_matrix_service = node.advertiseService("get_soft_iron_matrix", + &Microstrain::get_soft_iron_matrix, this); + ros::ServiceServer get_sensor_vehicle_frame_trans_service = node.advertiseService("get_sensor_vehicle_frame_trans", + &Microstrain::get_sensor_vehicle_frame_trans, this); + ros::ServiceServer get_complementary_filter_service = node.advertiseService("get_complementary_filter", + &Microstrain::get_complementary_filter, this); + ros::ServiceServer set_reference_position_service = node.advertiseService("set_reference_position", + &Microstrain::set_reference_position, this); + ros::ServiceServer get_reference_position_service = node.advertiseService("get_reference_position", + &Microstrain::get_reference_position, this); + ros::ServiceServer set_coning_sculling_comp_service = node.advertiseService("set_coning_sculling_comp", + &Microstrain::set_coning_sculling_comp, this); + ros::ServiceServer get_coning_sculling_comp_service = node.advertiseService("get_coning_sculling_comp", + &Microstrain::get_coning_sculling_comp, this); + ros::ServiceServer set_estimation_control_flags_service = node.advertiseService("set_estimation_control_flags", + &Microstrain::set_estimation_control_flags, this); + ros::ServiceServer get_estimation_control_flags_service = node.advertiseService("get_estimation_control_flags", + &Microstrain::get_estimation_control_flags, this); + ros::ServiceServer set_dynamics_mode_service = node.advertiseService("set_dynamics_mode", + &Microstrain::set_dynamics_mode, this); + ros::ServiceServer get_basic_status_service = node.advertiseService("get_basic_status", + &Microstrain::get_basic_status, this); + ros::ServiceServer get_diagnostic_report_service = node.advertiseService("get_diagnostic_report", + &Microstrain::get_diagnostic_report, this); + ros::ServiceServer set_zero_angle_update_threshold_service = node.advertiseService("set_zero_angle_update_threshold", + &Microstrain::set_zero_angle_update_threshold, this); + ros::ServiceServer get_zero_angle_update_threshold_service = node.advertiseService("get_zero_angle_update_threshold", + &Microstrain::get_zero_angle_update_threshold, this); + ros::ServiceServer set_tare_orientation_service = node.advertiseService("set_tare_orientation", + &Microstrain::set_tare_orientation, this); + ros::ServiceServer set_accel_noise_service = node.advertiseService("set_accel_noise", + &Microstrain::set_accel_noise, this); + ros::ServiceServer get_accel_noise_service = node.advertiseService("get_accel_noise", + &Microstrain::get_accel_noise, this); + ros::ServiceServer set_gyro_noise_service = node.advertiseService("set_gyro_noise", + &Microstrain::set_gyro_noise, this); + ros::ServiceServer get_gyro_noise_service = node.advertiseService("get_gyro_noise", + &Microstrain::get_gyro_noise, this); + ros::ServiceServer set_mag_noise_service = node.advertiseService("set_mag_noise", + &Microstrain::set_mag_noise, this); + ros::ServiceServer get_mag_noise_service = node.advertiseService("get_mag_noise", + &Microstrain::get_mag_noise, this); + ros::ServiceServer set_gyro_bias_model_service = node.advertiseService("set_gyro_bias_model", + &Microstrain::set_gyro_bias_model, this); + ros::ServiceServer get_gyro_bias_model_service = node.advertiseService("get_gyro_bias_model", + &Microstrain::get_gyro_bias_model, this); + ros::ServiceServer get_accel_adaptive_vals_service = node.advertiseService("get_accel_adaptive_vals", + &Microstrain::get_accel_adaptive_vals, this); + ros::ServiceServer set_mag_adaptive_vals_service = node.advertiseService("set_mag_adaptive_vals", + &Microstrain::set_mag_adaptive_vals, this); + ros::ServiceServer get_mag_adaptive_vals_service = node.advertiseService("get_mag_adaptive_vals", + &Microstrain::get_mag_adaptive_vals, this); + ros::ServiceServer set_mag_dip_adaptive_vals_service = node.advertiseService("set_mag_dip_adaptive_vals", + &Microstrain::set_mag_dip_adaptive_vals, this); + ros::ServiceServer get_accel_bias_model_service = node.advertiseService("get_accel_bias_model", + &Microstrain::get_accel_bias_model, this); + ros::ServiceServer get_mag_dip_adaptive_vals_service = node.advertiseService("get_mag_dip_adaptive_vals", + &Microstrain::get_mag_dip_adaptive_vals, this); + ros::ServiceServer get_sensor_vehicle_frame_offset_service = node.advertiseService("get_sensor_vehicle_frame_offset", + &Microstrain::get_sensor_vehicle_frame_offset, this); + ros::ServiceServer get_dynamics_mode_service = node.advertiseService("get_dynamics_mode", + &Microstrain::get_dynamics_mode, 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_FATAL("Couldn't open serial port! Is it plugged in?"); } @@ -225,7 +273,7 @@ namespace Microstrain //////////////////////////////////////// // Device setup - float dT=0.5; // common sleep time after setup communications + float dT = 0.5; // common sleep time after setup communications if (device_setup) { // Put device into standard mode - we never really use "direct mode" @@ -233,7 +281,8 @@ namespace Microstrain device_descriptors_size = 128*2; com_mode = MIP_SDK_GX4_45_IMU_STANDARD_MODE; start = clock(); - 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) { if (clock() - start > 5000) { @@ -242,10 +291,11 @@ namespace Microstrain } } - //Verify device mode setting + // Verify device mode setting ROS_INFO("Verify comm's mode"); start = clock(); - 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) { if (clock() - start > 5000) { @@ -257,14 +307,14 @@ namespace Microstrain ROS_INFO("Sleep for a second..."); ros::Duration(dT).sleep(); ROS_INFO("Right mode?"); - if(com_mode != MIP_SDK_GX4_45_IMU_STANDARD_MODE) + if (com_mode != MIP_SDK_GX4_45_IMU_STANDARD_MODE) { ROS_ERROR("Appears we didn't get into standard mode!"); } - //Get device info + // Get device info start = clock(); - while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) + while (mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -273,72 +323,77 @@ namespace Microstrain } } - //Get device model name + // Get device model name memset(temp_string, 0, 20*sizeof(char)); memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2); ROS_INFO("Model Name => %s\n", temp_string); std::string model_name; - for(int i = 6; i < 20; i++) + for (int i = 6; i < 20; i++) { model_name += temp_string[i]; } - //Set device model flag + // Set device model flag model_name = model_name.c_str(); - if(model_name == GX5_45_DEVICE) + if (model_name == GX5_45_DEVICE) { GX5_45 = true; } - if(model_name == GX5_35_DEVICE) + if (model_name == GX5_35_DEVICE) { GX5_35 = true; } - if(model_name == GX5_25_DEVICE) + if (model_name == GX5_25_DEVICE) { GX5_25 = true; } - if(model_name == GX5_15_DEVICE) + if (model_name == GX5_15_DEVICE) { GX5_15 = true; } - //Set GPS publishing to true if IMU model has GPS - if(GX5_45 || GX5_35) + // Set GPS publishing to true if IMU model has GPS + if (GX5_45 || GX5_35) { - private_nh.param("publish_gps",publish_gps_, true); - private_nh.param("publish_odom",publish_odom_, true); + private_nh.param("publish_gps", publish_gps_, true); + private_nh.param("publish_odom", publish_odom_, true); } else { - private_nh.param("publish_gps",publish_gps_, false); - private_nh.param("publish_odom",publish_odom_, false); + private_nh.param("publish_gps", publish_gps_, false); + private_nh.param("publish_odom", publish_odom_, false); } - if(publish_gps_) - gps_pub_ = node.advertise("gps/fix",100); + if (publish_gps_) + { + gps_pub_ = node.advertise("gps/fix", 100); + } - if(publish_odom_) + if (publish_odom_) { - nav_pub_ = node.advertise("nav/odom",100); - nav_status_pub_ = node.advertise("nav/status",100); + nav_pub_ = node.advertise("nav/odom", 100); + nav_status_pub_ = node.advertise("nav/status", 100); } // Setup device callbacks - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_FILTER_DATA_SET, this, &filter_packet_callback_wrapper) != MIP_INTERFACE_OK) + if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_FILTER_DATA_SET, + this, &filter_packet_callback_wrapper) != MIP_INTERFACE_OK) { ROS_FATAL("Can't setup filter callback!"); return; } - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET, this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) + if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET, + this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK) { ROS_FATAL("Can't setup ahrs callbacks!"); return; } - if(mip_interface_add_descriptor_set_callback(&device_interface_, MIP_GPS_DATA_SET, this, &gps_packet_callback_wrapper) != MIP_INTERFACE_OK) + if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_GPS_DATA_SET, + this, &gps_packet_callback_wrapper) != MIP_INTERFACE_OK) { ROS_FATAL("Can't setup gpscallbacks!"); return; @@ -347,7 +402,7 @@ namespace Microstrain // Put into idle mode ROS_INFO("Idling Device: Stopping data streams and/or waking from sleep"); start = clock(); - while(mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK) + while (mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -362,9 +417,9 @@ namespace Microstrain if (publish_imu_) { start = clock(); - while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_get_ahrs_base_rate function timed out."); break; @@ -374,8 +429,8 @@ namespace Microstrain ROS_INFO("AHRS Base Rate => %d Hz", base_rate); ros::Duration(dT).sleep(); // Deterimine decimation to get close to goal rate - u8 imu_decimation = (u8)((float)base_rate/ (float)imu_rate_); - ROS_INFO("AHRS decimation set to %#04X",imu_decimation); + u8 imu_decimation = (u8)(static_cast(base_rate)/ static_cast(imu_rate_)); + ROS_INFO("AHRS decimation set to %#04X", imu_decimation); // AHRS Message Format // Set message format @@ -383,15 +438,17 @@ namespace Microstrain 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_decimation[0] = imu_decimation;//0x32; - data_stream_format_decimation[1] = imu_decimation;//0x32; - data_stream_format_decimation[2] = imu_decimation;//0x32; + data_stream_format_decimation[0] = imu_decimation; // 0x32; + data_stream_format_decimation[1] = imu_decimation; // 0x32; + data_stream_format_decimation[2] = imu_decimation; // 0x32; data_stream_format_num_entries = 3; - + start = clock(); - 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) + 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) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); break; @@ -402,9 +459,11 @@ namespace Microstrain // Poll to verify ROS_INFO("Poll AHRS data to verify"); start = clock(); - 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) + 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) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_poll_ahrs function timed out."); break; @@ -417,9 +476,10 @@ namespace Microstrain { ROS_INFO("Saving AHRS data settings"); start = clock(); - while(mip_3dm_cmd_ahrs_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_ahrs_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out."); break; @@ -430,11 +490,12 @@ namespace Microstrain // Declination Source // Set declination - ROS_INFO("Setting declination source to %#04X",declination_source_u8); + ROS_INFO("Setting declination source to %#04X", declination_source_u8); start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK) + while (mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_declination_source function timed out."); break; @@ -442,19 +503,20 @@ namespace Microstrain } ros::Duration(dT).sleep(); - //Read back the declination source + // Read back the declination source ROS_INFO("Reading back declination source"); start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK) + while (mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_declination_source function timed out."); break; } } - if(declination_source_u8 == readback_declination_source) + if (declination_source_u8 == readback_declination_source) { ROS_INFO("Success: Declination source set to %#04X", declination_source_u8); } @@ -468,9 +530,10 @@ namespace Microstrain { ROS_INFO("Saving declination source settings to EEPROM"); start = clock(); - while(mip_filter_declination_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + while (mip_filter_declination_source(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_declination_source function timed out."); break; @@ -479,16 +542,16 @@ namespace Microstrain ros::Duration(dT).sleep(); } - } // end of AHRS setup + } // end of AHRS setup // GPS Setup if (publish_gps_) { start = clock(); - while(mip_3dm_cmd_get_gps_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_get_gps_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_get_gps_base_rate function timed out."); break; @@ -496,7 +559,7 @@ namespace Microstrain } ROS_INFO("GPS Base Rate => %d Hz", base_rate); - u8 gps_decimation = (u8)((float)base_rate/ (float)gps_rate_); + u8 gps_decimation = (u8)(static_cast(base_rate)/ static_cast(gps_rate_)); ros::Duration(dT).sleep(); ////////// GPS Message Format @@ -505,13 +568,15 @@ namespace Microstrain data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS; data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY; data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME; - data_stream_format_decimation[0] = gps_decimation; //0x01; //0x04; - data_stream_format_decimation[1] = gps_decimation; //0x01; //0x04; - data_stream_format_decimation[2] = gps_decimation; //0x01; //0x04; + data_stream_format_decimation[0] = gps_decimation; // 0x01; // 0x04; + data_stream_format_decimation[1] = gps_decimation; // 0x01; // 0x04; + data_stream_format_decimation[2] = gps_decimation; // 0x01; // 0x04; data_stream_format_num_entries = 3; start = clock(); - while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gps_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries, + data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -526,9 +591,10 @@ namespace Microstrain { ROS_INFO("Saving GPS data settings"); start = clock(); - while(mip_3dm_cmd_gps_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gps_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_gps_message_format function timed out."); break; @@ -536,15 +602,15 @@ namespace Microstrain } ros::Duration(dT).sleep(); } - } // end of GPS setup + } // end of GPS setup // Filter setup if (publish_odom_) { start = clock(); - while(mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_get_filter_base_rate function timed out."); break; @@ -552,7 +618,7 @@ namespace Microstrain } ROS_INFO("FILTER Base Rate => %d Hz", base_rate); - u8 nav_decimation = (u8)((float)base_rate/ (float)nav_rate_); + u8 nav_decimation = (u8)(static_cast(base_rate)/ static_cast(nav_rate_)); ros::Duration(dT).sleep(); ////////// Filter Message Format @@ -560,27 +626,29 @@ namespace Microstrain ROS_INFO("Setting Filter stream format"); data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS; data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL; - //data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; + // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES; data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_QUATERNION; data_stream_format_descriptors[3] = MIP_FILTER_DATA_POS_UNCERTAINTY; data_stream_format_descriptors[4] = MIP_FILTER_DATA_VEL_UNCERTAINTY; data_stream_format_descriptors[5] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER; data_stream_format_descriptors[6] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE; data_stream_format_descriptors[7] = MIP_FILTER_DATA_FILTER_STATUS; - data_stream_format_decimation[0] = nav_decimation; //0x32; - data_stream_format_decimation[1] = nav_decimation; //0x32; - data_stream_format_decimation[2] = nav_decimation; //0x32; - data_stream_format_decimation[3] = nav_decimation; //0x32; - data_stream_format_decimation[4] = nav_decimation; //0x32; - data_stream_format_decimation[5] = nav_decimation; //0x32; - data_stream_format_decimation[6] = nav_decimation; //0x32; - data_stream_format_decimation[7] = nav_decimation; //0x32; + data_stream_format_decimation[0] = nav_decimation; // 0x32; + data_stream_format_decimation[1] = nav_decimation; // 0x32; + data_stream_format_decimation[2] = nav_decimation; // 0x32; + data_stream_format_decimation[3] = nav_decimation; // 0x32; + data_stream_format_decimation[4] = nav_decimation; // 0x32; + data_stream_format_decimation[5] = nav_decimation; // 0x32; + data_stream_format_decimation[6] = nav_decimation; // 0x32; + data_stream_format_decimation[7] = nav_decimation; // 0x32; data_stream_format_num_entries = 8; start = clock(); - 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) + 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) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); break; @@ -591,9 +659,11 @@ namespace Microstrain // Poll to verify ROS_INFO("Poll filter data to test stream"); start = clock(); - 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) + 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) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_poll_filter function timed out."); break; @@ -606,9 +676,10 @@ namespace Microstrain { ROS_INFO("Saving Filter data settings"); start = clock(); - while(mip_3dm_cmd_filter_message_format(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL,NULL) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_filter_message_format(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_filter_message_format function timed out."); break; @@ -619,11 +690,12 @@ namespace Microstrain // Dynamics Mode // Set dynamics mode - ROS_INFO("Setting dynamics mode to %#04X",dynamics_mode); + ROS_INFO("Setting dynamics mode to %#04X", dynamics_mode); start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); break; @@ -637,9 +709,10 @@ namespace Microstrain // Read the settings back ROS_INFO("Reading back dynamics mode setting"); start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); break; @@ -647,20 +720,21 @@ namespace Microstrain } ros::Duration(dT).sleep(); - if(dynamics_mode == readback_dynamics_mode) - ROS_INFO("Success: Dynamics mode setting is: %#04X",readback_dynamics_mode); + 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); + dynamics_mode, readback_dynamics_mode); } if (save_settings) { ROS_INFO("Saving dynamics mode settings to EEPROM"); start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); break; @@ -669,40 +743,43 @@ namespace Microstrain ros::Duration(dT).sleep(); } - //Set heading Source + // Set heading Source ROS_INFO("Set heading source to internal mag."); heading_source = 0x1; - ROS_INFO("Setting heading source to %#04X",heading_source); + ROS_INFO("Setting heading source to %#04X", heading_source); start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK) + while (mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_heading_source function timed out."); break; } } - //Read back heading source + // Read back heading source ros::Duration(dT).sleep(); ROS_INFO("Read back heading source..."); start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_headingsource)!= MIP_INTERFACE_OK) + while (mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_headingsource)!= MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_heading_source function timed out."); break; } } - ROS_INFO("Heading source = %#04X",readback_headingsource); + ROS_INFO("Heading source = %#04X", readback_headingsource); ros::Duration(dT).sleep(); if (save_settings) { ROS_INFO("Saving heading source to EEPROM"); start = clock(); - while(mip_filter_heading_source(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL)!= MIP_INTERFACE_OK) + while (mip_filter_heading_source(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL)!= MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -715,12 +792,13 @@ namespace Microstrain } // end of Filter setup // Set auto-initialization based on ROS parameter - ROS_INFO("Setting auto-initinitalization to: %#04X",auto_init); + ROS_INFO("Setting auto-initinitalization to: %#04X", auto_init); auto_init_u8 = auto_init; // convert bool to u8 start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &auto_init_u8) != MIP_INTERFACE_OK) + while (mip_filter_auto_initialization(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &auto_init_u8) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_auto_initialization function timed out."); break; @@ -733,9 +811,10 @@ namespace Microstrain // Read the settings back ROS_INFO("Reading back auto-initialization value"); start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_auto_init)!= MIP_INTERFACE_OK) + while (mip_filter_auto_initialization(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_auto_init)!= MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_auto_initialization function timed out."); break; @@ -744,19 +823,21 @@ namespace Microstrain ros::Duration(dT).sleep(); if (auto_init == readback_auto_init) - ROS_INFO("Success: Auto init. setting is: %#04X",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); + auto_init, readback_auto_init); } if (save_settings) { ROS_INFO("Saving auto init. settings to EEPROM"); start = clock(); - while(mip_filter_auto_initialization(&device_interface_, MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) + while (mip_filter_auto_initialization(&device_interface_, + MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_auto_initialization function timed out."); break; @@ -772,9 +853,10 @@ namespace Microstrain ROS_INFO("Enabling AHRS stream"); enable = 0x01; start = clock(); - 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) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); break; @@ -788,9 +870,10 @@ namespace Microstrain ROS_INFO("Enabling Filter stream"); enable = 0x01; start = clock(); - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_continuous_data_stream(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); break; @@ -804,9 +887,10 @@ namespace Microstrain ROS_INFO("Enabling GPS stream"); enable = 0x01; start = clock(); - while(mip_3dm_cmd_continuous_data_stream(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_continuous_data_stream(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out."); break; @@ -820,14 +904,14 @@ namespace Microstrain else { ROS_INFO("Skipping device setup and listing for existing streams"); - } // end of device_setup + } // end of device_setup // Reset filter - should be for either the KF or CF ROS_INFO("Reset filter"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reset_filter function timed out."); break; @@ -840,50 +924,48 @@ namespace Microstrain int max_rate = 1; if (publish_imu_) { - max_rate = std::max(max_rate,imu_rate_); + max_rate = std::max(max_rate, imu_rate_); } if (publish_gps_) { - max_rate = std::max(max_rate,gps_rate_); + max_rate = std::max(max_rate, gps_rate_); } if (publish_odom_) { - max_rate = std::max(max_rate,nav_rate_); + max_rate = std::max(max_rate, nav_rate_); } - int spin_rate = std::min(3*max_rate,1000); - ROS_INFO("Setting spin rate to <%d>",spin_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 microstrain_mips::RosDiagnosticUpdater ros_diagnostic_updater(this); while (ros::ok()) { - //Update the parser (this function reads the port and parses the bytes + // Update the parser (this function reads the port and parses the bytes mip_interface_update(&device_interface_); - if(GX5_25) + if (GX5_25) { device_status_callback(); } ros::spinOnce(); // take care of service requests. r.sleep(); - - } // end loop + } // end loop // close serial port mip_sdk_port_close(device_interface_.port_handle); - - } // End of ::run() + } // End of ::run() bool Microstrain::reset_callback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) { ROS_INFO("Reseting the filter"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reset_filter function timed out."); break; @@ -892,33 +974,38 @@ namespace Microstrain return true; } - //Services to get/set values on devices - //Set accel bias values - bool Microstrain::set_accel_bias(microstrain_mips::SetAccelBias::Request &req, microstrain_mips::SetAccelBias::Response &res) + // Services to get/set values on devices + // Set accel bias values + bool Microstrain::set_accel_bias(microstrain_mips::SetAccelBias::Request &req, + microstrain_mips::SetAccelBias::Response &res) { ROS_INFO("Setting accel bias values"); memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_accel_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); break; } } - ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); + ROS_INFO("Accel bias vector values are: %f %f %f", + field_data[0], field_data[1], field_data[2]); + ROS_INFO("Client request values are: %.2f %.2f %.2f", + req.bias.x, req.bias.y, req.bias.z); field_data[0] = req.bias.x; field_data[1] = req.bias.y; field_data[2] = req.bias.z; start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_accel_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); break; @@ -927,68 +1014,78 @@ namespace Microstrain memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_accel_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); break; } } - ROS_INFO("New accel bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("New accel bias vector values are: %.2f %.2f %.2f", + field_data[0], field_data[1], field_data[2]); res.success = true; return true; } - //Get accel bias values - bool Microstrain::get_accel_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get accel bias values + bool Microstrain::get_accel_bias(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { ROS_INFO("Getting accel bias values"); memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_accel_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_accel_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_accel_bias function timed out."); break; } } - ROS_INFO("Accel bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Accel bias vector values are: %f %f %f", + field_data[0], field_data[1], field_data[2]); res.success = true; return true; } - //Set gyro bias values - bool Microstrain::set_gyro_bias(microstrain_mips::SetGyroBias::Request &req, microstrain_mips::SetGyroBias::Response &res) + // Set gyro bias values + bool Microstrain::set_gyro_bias(microstrain_mips::SetGyroBias::Request &req, + microstrain_mips::SetGyroBias::Response &res) { ROS_INFO("Setting gyro bias values"); memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gyro_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); break; } } - ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]); - ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z); + ROS_INFO("Gyro bias vector values are: %f %f %f", + field_data[0], field_data[1], field_data[2]); + ROS_INFO("Client request values are: %.2f %.2f %.2f", + req.bias.x, req.bias.y, req.bias.z); field_data[0] = req.bias.x; field_data[1] = req.bias.y; field_data[2] = req.bias.z; start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gyro_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); break; @@ -996,30 +1093,34 @@ namespace Microstrain } memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gyro_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); break; } } - ROS_INFO("New gyro bias vector values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]); + ROS_INFO("New gyro bias vector values are: %.2f %.2f %.2f", + field_data[0], field_data[1], field_data[2]); res.success = true; return true; } - //Get gyro bias values - bool Microstrain::get_gyro_bias(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get gyro bias values + bool Microstrain::get_gyro_bias(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { ROS_INFO("Getting gyro bias values"); memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_gyro_bias(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_gyro_bias(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_gyro_bias function timed out."); break; @@ -1031,10 +1132,11 @@ namespace Microstrain return true; } - //Set hard iron values - bool Microstrain::set_hard_iron_values(microstrain_mips::SetHardIronValues::Request &req, microstrain_mips::SetHardIronValues::Response &res) + // Set hard iron values + bool Microstrain::set_hard_iron_values(microstrain_mips::SetHardIronValues::Request &req, + microstrain_mips::SetHardIronValues::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1045,9 +1147,10 @@ namespace Microstrain float field_data[3] = {0}; start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hard_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); break; @@ -1062,9 +1165,10 @@ namespace Microstrain field_data[2] = req.bias.z; start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hard_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); break; @@ -1073,9 +1177,10 @@ namespace Microstrain memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hard_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); break; @@ -1087,11 +1192,10 @@ namespace Microstrain return true; } - //Get hard iron values + // Get hard iron values bool Microstrain::get_hard_iron_values(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1102,9 +1206,10 @@ namespace Microstrain memset(field_data, 0, 3*sizeof(float)); start = clock(); - while(mip_3dm_cmd_hard_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hard_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hard_iron function timed out."); break; @@ -1117,13 +1222,13 @@ namespace Microstrain } - //Get device report + // Get device report bool Microstrain::device_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { start = clock(); - while(mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) + while (mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_base_cmd_get_device_info function timed out."); break; @@ -1149,37 +1254,40 @@ namespace Microstrain memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2); ROS_INFO("Options => %s\n", temp_string); - ROS_INFO("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000, (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100); + ROS_INFO("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000, + (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100); res.success = true; return true; } - //Capture gyro bias values + // Capture gyro bias values bool Microstrain::gyro_bias_capture(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { memset(field_data, 0, 3*sizeof(float)); ROS_INFO("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n"); - duration = 5000; //milliseconds + duration = 5000; // milliseconds start = clock(); - while(mip_3dm_cmd_capture_gyro_bias(&device_interface_, duration, field_data) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_capture_gyro_bias(&device_interface_, duration, field_data) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_capture_gyro_bias function timed out."); break; } } - ROS_INFO("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", field_data[0], field_data[1], field_data[2]); + ROS_INFO("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", + field_data[0], field_data[1], field_data[2]); res.success = true; return true; } - //Set soft iron matrix values - bool Microstrain::set_soft_iron_matrix(microstrain_mips::SetSoftIronMatrix::Request &req, microstrain_mips::SetSoftIronMatrix::Response &res) + // Set soft iron matrix values + bool Microstrain::set_soft_iron_matrix(microstrain_mips::SetSoftIronMatrix::Request &req, + microstrain_mips::SetSoftIronMatrix::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1202,56 +1310,69 @@ namespace Microstrain soft_iron[8] = req.soft_iron_3.z; start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_soft_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); break; } } - //Read back the soft iron matrix values + // Read back the soft iron matrix values start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_soft_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); break; } } - if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) && - (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) && - (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) && - (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) && - (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) && - (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) && - (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) && - (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) && - (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001)) + if ((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) && + (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) && + (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) && + (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) && + (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) && + (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) && + (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) && + (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) && + (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001)) { ROS_INFO("Soft iron matrix values successfully set.\n"); - ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); - ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", + soft_iron[0], soft_iron[1], soft_iron[2], + soft_iron[3], soft_iron[4], soft_iron[5], + soft_iron[6], soft_iron[7], soft_iron[8]); + ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", + soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], + soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5], + soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); } else { ROS_INFO("ERROR: Failed to set hard iron values!!!\n"); - ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]); - ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n", + soft_iron[0], soft_iron[1], soft_iron[2], + soft_iron[3], soft_iron[4], soft_iron[5], + soft_iron[6], soft_iron[7], soft_iron[8]); + ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n", + soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], + soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5], + soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); } res.success = true; return true; } - //Get soft iron matrix values - bool Microstrain::get_soft_iron_matrix(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get soft iron matrix values + bool Microstrain::get_soft_iron_matrix(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1264,23 +1385,27 @@ namespace Microstrain ROS_INFO("Getting the soft iron matrix values\n"); start = clock(); - while(mip_3dm_cmd_soft_iron(&device_interface_, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_soft_iron(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_soft_iron function timed out."); break; } } - ROS_INFO("Soft iron matrix values: [%f %f %f][%f %f %f][%f %f %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], - soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); + ROS_INFO("Soft iron matrix values: [%f %f %f][%f %f %f][%f %f %f]\n", + soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], + soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5], + soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]); res.success = true; return true; } - //Set complementary filter values - bool Microstrain::set_complementary_filter(microstrain_mips::SetComplementaryFilter::Request &req, microstrain_mips::SetComplementaryFilter::Response &res) + // Set complementary filter values + bool Microstrain::set_complementary_filter(microstrain_mips::SetComplementaryFilter::Request &req, + microstrain_mips::SetComplementaryFilter::Response &res) { ROS_INFO("Setting the complementary filter values\n"); @@ -1290,73 +1415,90 @@ namespace Microstrain comp_filter_command.up_compensation_time_constant = req.up_comp_time_const; start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_complementary_filter_settings(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); break; } } - //Read back the complementary filter values + // Read back the complementary filter values start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_complementary_filter_settings(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); break; } } - if((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) && - (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) && - (abs(comp_filter_command.north_compensation_time_constant - comp_filter_readback.north_compensation_time_constant) < 0.001) && - (abs(comp_filter_command.up_compensation_time_constant - comp_filter_readback.up_compensation_time_constant) < 0.001)) + if ((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) && + (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) && + (abs(comp_filter_command.north_compensation_time_constant - + comp_filter_readback.north_compensation_time_constant) < 0.001) && + (abs(comp_filter_command.up_compensation_time_constant - + comp_filter_readback.up_compensation_time_constant) < 0.001)) { ROS_INFO("Complementary filter values successfully set.\n"); - ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", + comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, + comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", + comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, + comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); } else { ROS_INFO("ERROR: Failed to set complementary filter values!!!\n"); - ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", + comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable, + comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant); + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", + comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, + comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); } res.success = true; return true; } - //Get complementary filter values - bool Microstrain::get_complementary_filter(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get complementary filter values + bool Microstrain::get_complementary_filter(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - //Read back the complementary filter values + // Read back the complementary filter values start = clock(); - while(mip_3dm_cmd_complementary_filter_settings(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_complementary_filter_settings(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out."); break; } } - ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); + ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", + comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable, + comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant); res.success = true; return true; } - //Initialize filter with Euler angles - bool Microstrain::set_filter_euler(microstrain_mips::SetFilterEuler::Request &req, microstrain_mips::SetFilterEuler::Response &res) + // Initialize filter with Euler angles + bool Microstrain::set_filter_euler(microstrain_mips::SetFilterEuler::Request &req, + microstrain_mips::SetFilterEuler::Response &res) { memset(angles, 0, 3*sizeof(float)); ROS_INFO("Resetting the Filter\n"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reset_filter function timed out."); break; @@ -1370,9 +1512,9 @@ namespace Microstrain angles[2] = req.angle.z; start = clock(); - while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) + while (mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_set_init_attitude function timed out."); break; @@ -1383,15 +1525,16 @@ namespace Microstrain return true; } - //Set filter with heading angle - bool Microstrain::set_filter_heading(microstrain_mips::SetFilterHeading::Request &req, microstrain_mips::SetFilterHeading::Response &res) + // Set filter with heading angle + bool Microstrain::set_filter_heading(microstrain_mips::SetFilterHeading::Request &req, + microstrain_mips::SetFilterHeading::Response &res) { ROS_INFO("Resetting the Filter\n"); start = clock(); - while(mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) + while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reset_filter function timed out."); break; @@ -1402,9 +1545,9 @@ namespace Microstrain heading_angle = req.angle; start = clock(); - while(mip_filter_set_init_heading(&device_interface_, heading_angle) != MIP_INTERFACE_OK) + while (mip_filter_set_init_heading(&device_interface_, heading_angle) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_set_init_heading function timed out."); break; @@ -1416,10 +1559,11 @@ namespace Microstrain } - //Set sensor to vehicle frame transformation - bool Microstrain::set_sensor_vehicle_frame_trans(microstrain_mips::SetSensorVehicleFrameTrans::Request &req, microstrain_mips::SetSensorVehicleFrameTrans::Response &res) + // Set sensor to vehicle frame transformation + bool Microstrain::set_sensor_vehicle_frame_trans(microstrain_mips::SetSensorVehicleFrameTrans::Request &req, + microstrain_mips::SetSensorVehicleFrameTrans::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1436,47 +1580,53 @@ namespace Microstrain angles[2] = req.angle.z; start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_tranformation(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } } - //Read back the transformation + // Read back the transformation start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_tranformation(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } } - if((abs(readback_angles[0]-angles[0]) < 0.001) && + if ((abs(readback_angles[0]-angles[0]) < 0.001) && (abs(readback_angles[1]-angles[1]) < 0.001) && (abs(readback_angles[2]-angles[2]) < 0.001)) { ROS_INFO("Transformation successfully set.\n"); - ROS_INFO("New angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); + ROS_INFO("New angles: %f roll %f pitch %f yaw\n", + readback_angles[0], readback_angles[1], readback_angles[2]); } else { ROS_INFO("ERROR: Failed to set transformation!!!\n"); - ROS_INFO("Sent angles: %f roll %f pitch %f yaw\n", angles[0], angles[1], angles[2]); - ROS_INFO("Returned angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); + ROS_INFO("Sent angles: %f roll %f pitch %f yaw\n", + angles[0], angles[1], angles[2]); + ROS_INFO("Returned angles: %f roll %f pitch %f yaw\n", + readback_angles[0], readback_angles[1], readback_angles[2]); } res.success = true; return true; } - //Get sensor to vehicle frame transformation - bool Microstrain::get_sensor_vehicle_frame_trans(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get sensor to vehicle frame transformation + bool Microstrain::get_sensor_vehicle_frame_trans(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -1484,25 +1634,28 @@ namespace Microstrain } memset(readback_angles, 0, 3*sizeof(float)); - //Read back the transformation + // Read back the transformation start = clock(); - while(mip_filter_sensor2vehicle_tranformation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_tranformation(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out."); break; } } - ROS_INFO("Sensor Vehicle Frame Transformation Angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]); + ROS_INFO("Sensor Vehicle Frame Transformation Angles: %f roll %f pitch %f yaw\n", + readback_angles[0], readback_angles[1], readback_angles[2]); res.success = true; return true; } - //Set reference position - bool Microstrain::set_reference_position(microstrain_mips::SetReferencePosition::Request &req, microstrain_mips::SetReferencePosition::Response &res) + // Set reference position + bool Microstrain::set_reference_position(microstrain_mips::SetReferencePosition::Request &req, + microstrain_mips::SetReferencePosition::Response &res) { ROS_INFO("Setting reference Position\n"); @@ -1516,30 +1669,32 @@ namespace Microstrain reference_position_command[2] = req.position.z; start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &reference_position_enable_command, reference_position_command) != MIP_INTERFACE_OK) + while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, + &reference_position_enable_command, reference_position_command) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reference_position function timed out."); break; } } - //Read back the reference position + // Read back the reference position start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) + while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, + &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reference_position function timed out."); break; } } - if((reference_position_enable_command == reference_position_enable_readback) && - (abs(reference_position_command[0] - reference_position_readback[0]) < 0.001) && - (abs(reference_position_command[1] - reference_position_readback[1]) < 0.001) && - (abs(reference_position_command[2] - reference_position_readback[2]) < 0.001)) + if ((reference_position_enable_command == reference_position_enable_readback) && + (abs(reference_position_command[0] - reference_position_readback[0]) < 0.001) && + (abs(reference_position_command[1] - reference_position_readback[1]) < 0.001) && + (abs(reference_position_command[2] - reference_position_readback[2]) < 0.001)) { ROS_INFO("Reference position successfully set\n"); } @@ -1552,37 +1707,41 @@ namespace Microstrain return true; } - //Get reference position + // Get reference position bool Microstrain::get_reference_position(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { ROS_INFO("Getting reference position"); memset(reference_position_readback, 0, 3*sizeof(double)); start = clock(); - while(mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) + while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ, + &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_reference_position function timed out."); break; } } - ROS_INFO("Reference position: Lat %f , Long %f, Alt %f", reference_position_readback[0], reference_position_readback[1], reference_position_readback[2]); + ROS_INFO("Reference position: Lat %f , Long %f, Alt %f", reference_position_readback[0], + reference_position_readback[1], reference_position_readback[2]); res.success = true; return true; } - //Enable or disable coning and sculling compensation - bool Microstrain::set_coning_sculling_comp(microstrain_mips::SetConingScullingComp::Request &req, microstrain_mips::SetConingScullingComp::Response &res) + // Enable or disable coning and sculling compensation + bool Microstrain::set_coning_sculling_comp(microstrain_mips::SetConingScullingComp::Request &req, + microstrain_mips::SetConingScullingComp::Response &res) { - if(req.enable == 0) + if (req.enable == 0) { ROS_INFO("Disabling Coning and Sculling compensation\n"); enable_flag = MIP_3DM_CONING_AND_SCULLING_DISABLE; start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; @@ -1591,9 +1750,10 @@ namespace Microstrain ROS_INFO("Reading Coning and Sculling compensation enabled state:\n"); start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; @@ -1601,26 +1761,28 @@ namespace Microstrain } ROS_INFO("%s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); } - else if(req.enable == 1) + else if (req.enable == 1) { ROS_INFO("Enabling Coning and Sculling compensation\n"); enable_flag = MIP_3DM_CONING_AND_SCULLING_ENABLE; start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } } - //Read back enable/disable value + // Read back enable/disable value ROS_INFO("Reading Coning and Sculling compensation enabled state:\n"); start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; @@ -1637,31 +1799,35 @@ namespace Microstrain return true; } - //Get coning and sculling compenastion enabled/disabled state + // Get coning and sculling compenastion enabled/disabled state bool Microstrain::get_coning_sculling_comp(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { start = clock(); - while(mip_3dm_cmd_coning_sculling_compensation(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out."); break; } } - ROS_INFO("Coning and Sculling compensation is: %s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); + ROS_INFO("Coning and Sculling compensation is: %s\n\n", + enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED"); res.success = true; return true; } - //Set estimation control filter flags - bool Microstrain::set_estimation_control_flags(microstrain_mips::SetEstimationControlFlags::Request &req, microstrain_mips::SetEstimationControlFlags::Response &res) + // Set estimation control filter flags + bool Microstrain::set_estimation_control_flags(microstrain_mips::SetEstimationControlFlags::Request &req, + microstrain_mips::SetEstimationControlFlags::Response &res) { estimation_control = req.flag; start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK) + while (mip_filter_estimation_control(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_estimation_control function timed out."); break; @@ -1669,9 +1835,10 @@ namespace Microstrain } start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) + while (mip_filter_estimation_control(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_estimation_control function timed out."); break; @@ -1684,13 +1851,15 @@ namespace Microstrain } - //Get estimatio control filter flags - bool Microstrain::get_estimation_control_flags(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get estimatio control filter flags + bool Microstrain::get_estimation_control_flags(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { start = clock(); - while(mip_filter_estimation_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) + while (mip_filter_estimation_control(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_estimation_control function timed out."); break; @@ -1703,17 +1872,19 @@ namespace Microstrain } - //Get device basic status. Variables in basic status struct change based on device model - bool Microstrain::get_basic_status(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get device basic status. Variables in basic status struct change based on device model + bool Microstrain::get_basic_status(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - //Use the basic status struct for the GX-25 - if(GX5_25) + // Use the basic status struct for the GX-25 + if (GX5_25) { u8 response_buffer[sizeof(gx4_25_basic_status_field)]; start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_BASIC_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, + GX4_25_BASIC_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); break; @@ -1721,12 +1892,14 @@ namespace Microstrain } ROS_INFO("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model); - ROS_INFO("Status Selector: \t\t\t\t%d\n", basic_field.status_selector);// == GX4_25_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report"); + ROS_INFO("Status Selector: \t\t\t\t%d\n", basic_field.status_selector); + // == GX4_25_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report"); ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", basic_field.status_flags); ROS_INFO("System state: \t\t\t\t\t%04u\n", basic_field.system_state); ROS_INFO("System Microsecond Timer Count: \t\t%lu ms\n\n", basic_field.system_timer_ms); } - else { + else + { ROS_INFO("Command not supported on this model"); } @@ -1734,17 +1907,19 @@ namespace Microstrain return true; } - //Get diagnostic status of device. Changes based on device model. - bool Microstrain::get_diagnostic_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get diagnostic status of device. Changes based on device model. + bool Microstrain::get_diagnostic_report(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - //Use GX5-25 device diagnostic struct - if(GX5_25) + // Use GX5-25 device diagnostic struct + if (GX5_25) { u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, + GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); break; @@ -1752,7 +1927,8 @@ namespace Microstrain } ROS_INFO("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model); - ROS_INFO("Status Selector: \t\t\t\t%d\n", diagnostic_field.status_selector);// == 114 ? "Basic Status Report" : "Diagnostic Status Report"); + ROS_INFO("Status Selector: \t\t\t\t%d\n", diagnostic_field.status_selector); + // == 114 ? "Basic Status Report" : "Diagnostic Status Report"); ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", diagnostic_field.status_flags); ROS_INFO("System Millisecond Timer Count: \t\t%lu ms\n", diagnostic_field.system_timer_ms); ROS_INFO("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE"); @@ -1766,10 +1942,9 @@ namespace Microstrain ROS_INFO("IMU Parser Errors: \t\t\t\t%lu Errors\n", diagnostic_field.imu_parser_errors); ROS_INFO("IMU Message Count: \t\t\t\t%lu Messages\n", diagnostic_field.imu_message_count); ROS_INFO("IMU Last Message Received: \t\t\t%lu ms\n", diagnostic_field.imu_last_message_ms); - } - //Unable to get this function working for the GX5-45 - else + // Unable to get this function working for the GX5-45 + else { ROS_INFO("Command not supported on this model"); } @@ -1778,71 +1953,80 @@ namespace Microstrain return true; } - //Set zero angular-rate update threshold - bool Microstrain::set_zero_angle_update_threshold(microstrain_mips::SetZeroAngleUpdateThreshold::Request &req, microstrain_mips::SetZeroAngleUpdateThreshold::Response &res) + // Set zero angular-rate update threshold + bool Microstrain::set_zero_angle_update_threshold(microstrain_mips::SetZeroAngleUpdateThreshold::Request &req, + microstrain_mips::SetZeroAngleUpdateThreshold::Response &res) { ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); - zero_update_control.threshold = req.threshold; // rads/s - zero_update_control.enable = req.enable; //enable zero-angular-rate update + zero_update_control.threshold = req.threshold; // rads/s + zero_update_control.enable = req.enable; // enable zero-angular-rate update - //Set ZUPT parameters + // Set ZUPT parameters start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK) + while (mip_filter_zero_angular_rate_update_control(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); break; } } - //Read back parameter settings + // Read back parameter settings start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) + while (mip_filter_zero_angular_rate_update_control(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); break; } } - if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold) + if (zero_update_control.enable != zero_update_readback.enable || + zero_update_control.threshold != zero_update_readback.threshold) ROS_INFO("ERROR configuring Zero Angular Rate Update.\n"); - ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); + ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", + zero_update_readback.enable, zero_update_readback.threshold); res.success = true; return true; } - //Get zero angular rate update threshold value - bool Microstrain::get_zero_angle_update_threshold(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get zero angular rate update threshold value + bool Microstrain::get_zero_angle_update_threshold(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { ROS_INFO("Setting Zero Angular-Rate-Update threshold\n"); - //Read back parameter settings + // Read back parameter settings start = clock(); - while(mip_filter_zero_angular_rate_update_control(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) + while (mip_filter_zero_angular_rate_update_control(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out."); break; } } - ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", zero_update_readback.enable, zero_update_readback.threshold); + ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s", + zero_update_readback.enable, zero_update_readback.threshold); res.success = true; return true; } - //Set tare orientation angle values - bool Microstrain::set_tare_orientation(microstrain_mips::SetTareOrientation::Request &req, microstrain_mips::SetTareOrientation::Response &res) + // Set tare orientation angle values + bool Microstrain::set_tare_orientation(microstrain_mips::SetTareOrientation::Request &req, + microstrain_mips::SetTareOrientation::Response &res) { - if(req.axis < 1 || req.axis > 7) + if (req.axis < 1 || req.axis > 7) { ROS_INFO("Value must be between 1-7. 1 = Roll, 2 = Pitch, 3 = Roll/Pitch, 4 = Yaw, 5 = Roll/Yaw, 6 = Pitch/Yaw, 7 = Roll/Pitch/Yaw"); res.success = false; @@ -1852,30 +2036,37 @@ namespace Microstrain int i = req.axis; start = clock(); - while(mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) + while (mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_set_init_attitude function timed out."); break; } } - //Wait for Filter to re-establish running state + // Wait for Filter to re-establish running state Sleep(5000); - //Cycle through axes combinations - if(mip_filter_tare_orientation(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK) + // Cycle through axes combinations + if (mip_filter_tare_orientation(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK) { ROS_INFO("ERROR: Failed Axis - "); - if(i & FILTER_TARE_ROLL_AXIS) + if (i & FILTER_TARE_ROLL_AXIS) + { ROS_INFO(" Roll Axis "); + } - if(i & FILTER_TARE_PITCH_AXIS) + if (i & FILTER_TARE_PITCH_AXIS) + { ROS_INFO(" Pitch Axis "); + } - if(i & FILTER_TARE_YAW_AXIS) + if (i & FILTER_TARE_YAW_AXIS) + { ROS_INFO(" Yaw Axis "); + } } else { @@ -1883,14 +2074,20 @@ namespace Microstrain ROS_INFO("Tared -"); - if(i & FILTER_TARE_ROLL_AXIS) + if (i & FILTER_TARE_ROLL_AXIS) + { ROS_INFO(" Roll Axis "); + } - if(i & FILTER_TARE_PITCH_AXIS) + if (i & FILTER_TARE_PITCH_AXIS) + { ROS_INFO(" Pitch Axis "); + } - if(i & FILTER_TARE_YAW_AXIS) + if (i & FILTER_TARE_YAW_AXIS) + { ROS_INFO(" Yaw Axis "); + } res.success = true; return true; @@ -1899,8 +2096,9 @@ namespace Microstrain Sleep(1000); } - //Set accel noise values - bool Microstrain::set_accel_noise(microstrain_mips::SetAccelNoise::Request &req, microstrain_mips::SetAccelNoise::Response &res) + // Set accel noise values + bool Microstrain::set_accel_noise(microstrain_mips::SetAccelNoise::Request &req, + microstrain_mips::SetAccelNoise::Response &res) { ROS_INFO("Setting the accel noise values\n"); @@ -1909,27 +2107,29 @@ namespace Microstrain noise[2] = req.noise.z; start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_noise function timed out."); break; } } - //Read back the accel noise values + // Read back the accel noise values start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_noise function timed out."); break; } } - if((abs(readback_noise[0]-noise[0]) < 0.001) && + if ((abs(readback_noise[0]-noise[0]) < 0.001) && (abs(readback_noise[1]-noise[1]) < 0.001) && (abs(readback_noise[2]-noise[2]) < 0.001)) { @@ -1938,34 +2138,40 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set accel noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Sent values: %f X %f Y %f Z\n", + noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); } res.success; return true; } - //Get accel noise values - bool Microstrain::get_accel_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get accel noise values + bool Microstrain::get_accel_noise(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { start = clock(); - while(mip_filter_accel_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_noise function timed out."); break; } } - ROS_INFO("Accel noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Accel noise values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); res.success = true; return true; } - //Set gyro noise values - bool Microstrain::set_gyro_noise(microstrain_mips::SetGyroNoise::Request &req, microstrain_mips::SetGyroNoise::Response &res) + // Set gyro noise values + bool Microstrain::set_gyro_noise(microstrain_mips::SetGyroNoise::Request &req, + microstrain_mips::SetGyroNoise::Response &res) { ROS_INFO("Setting the gyro noise values\n"); @@ -1974,27 +2180,29 @@ namespace Microstrain noise[2] = req.noise.z; start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_noise function timed out."); break; } } - //Read back the gyro noise values + // Read back the gyro noise values start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_noise function timed out."); break; } } - if((abs(readback_noise[0]-noise[0]) < 0.001) && + if ((abs(readback_noise[0]-noise[0]) < 0.001) && (abs(readback_noise[1]-noise[1]) < 0.001) && (abs(readback_noise[2]-noise[2]) < 0.001)) { @@ -2003,36 +2211,42 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set gyro noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Sent values: %f X %f Y %f Z\n", + noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); } res.success = true; return true; } - //Get gyro noise values - bool Microstrain::get_gyro_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get gyro noise values + bool Microstrain::get_gyro_noise(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { start = clock(); - while(mip_filter_gyro_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_noise function timed out."); break; } } - ROS_INFO("Gyro noise values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Gyro noise values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); res.success = true; return true; } - //Set magnetometer noise values - bool Microstrain::set_mag_noise(microstrain_mips::SetMagNoise::Request &req, microstrain_mips::SetMagNoise::Response &res) + // Set magnetometer noise values + bool Microstrain::set_mag_noise(microstrain_mips::SetMagNoise::Request &req, + microstrain_mips::SetMagNoise::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2046,27 +2260,29 @@ namespace Microstrain noise[2] = req.noise.z; start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) + while (mip_filter_mag_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_noise function timed out."); break; } } - //Read back the mag white noise values + // Read back the mag white noise values start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_mag_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_noise function timed out."); break; } } - if((abs(readback_noise[0] - noise[0]) < 0.001) && + if ((abs(readback_noise[0] - noise[0]) < 0.001) && (abs(readback_noise[1] - noise[1]) < 0.001) && (abs(readback_noise[2] - noise[2]) < 0.001)) { @@ -2075,18 +2291,21 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set mag noise values!!!\n"); - ROS_INFO("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Sent values: %f X %f Y %f Z\n", + noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); } res.success = true; return true; } - //Get magnetometer noise values - bool Microstrain::get_mag_noise(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get magnetometer noise values + bool Microstrain::get_mag_noise(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2094,23 +2313,26 @@ namespace Microstrain } start = clock(); - while(mip_filter_mag_noise(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_mag_noise(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_noise function timed out."); break; } } - ROS_INFO("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Returned values: %f X %f Y %f Z\n", + readback_noise[0], readback_noise[1], readback_noise[2]); res.success = true; return true; } - //Set gyro bias model - bool Microstrain::set_gyro_bias_model(microstrain_mips::SetGyroBiasModel::Request &req, microstrain_mips::SetGyroBiasModel::Response &res) + // Set gyro bias model + bool Microstrain::set_gyro_bias_model(microstrain_mips::SetGyroBiasModel::Request &req, + microstrain_mips::SetGyroBiasModel::Response &res) { ROS_INFO("Setting the gyro bias model values\n"); @@ -2123,27 +2345,29 @@ namespace Microstrain beta[2] = req.beta_vector.x; start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_bias_model(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_bias_model function timed out."); break; } } - //Read back the gyro bias model values + // Read back the gyro bias model values start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_bias_model(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_bias_model function timed out."); break; } } - if((abs(readback_noise[0]-noise[0]) < 0.001) && + if ((abs(readback_noise[0]-noise[0]) < 0.001) && (abs(readback_noise[1]-noise[1]) < 0.001) && (abs(readback_noise[2]-noise[2]) < 0.001) && (abs(readback_beta[0]-beta[0]) < 0.001) && @@ -2155,37 +2379,42 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set gyro bias model values!!!\n"); - ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], - readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + readback_beta[0], readback_beta[1], readback_beta[2], + readback_noise[0], readback_noise[1], readback_noise[2]); } res.success = true; return true; } - //Get gyro bias model + // Get gyro bias model bool Microstrain::get_gyro_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { start = clock(); - while(mip_filter_gyro_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_gyro_bias_model(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_gyro_bias_model function timed out."); break; } } - ROS_INFO("Gyro bias model values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Gyro bias model values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]); res.success = true; return true; } - //Get acces bias model - bool Microstrain::get_accel_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get acces bias model + bool Microstrain::get_accel_bias_model(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2196,7 +2425,8 @@ namespace Microstrain memset(readback_beta, 0, 3*sizeof(float)); start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_bias_model(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) { if (clock() - start > 5000) { @@ -2205,17 +2435,19 @@ namespace Microstrain } } - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], - readback_noise[0], readback_noise[1], readback_noise[2]); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + readback_beta[0], readback_beta[1], readback_beta[2], + readback_noise[0], readback_noise[1], readback_noise[2]); res.success = true; return true; } - //Set accel bias model - bool Microstrain::set_accel_bias_model(microstrain_mips::SetAccelBiasModel::Request &req, microstrain_mips::SetAccelBiasModel::Response &res) + // Set accel bias model + bool Microstrain::set_accel_bias_model(microstrain_mips::SetAccelBiasModel::Request &req, + microstrain_mips::SetAccelBiasModel::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2237,27 +2469,29 @@ namespace Microstrain beta[2] = req.beta_vector.x; start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_bias_model(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_bias_model function timed out."); break; } } - //Read back the accel bias model values + // Read back the accel bias model values start = clock(); - while(mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK) + while (mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ, + readback_beta, readback_noise) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_bias_model function timed out."); break; } } - if((abs(readback_noise[0]-noise[0]) < 0.001) && + if ((abs(readback_noise[0]-noise[0]) < 0.001) && (abs(readback_noise[1]-noise[1]) < 0.001) && (abs(readback_noise[2]-noise[2]) < 0.001) && (abs(readback_beta[0]-beta[0]) < 0.001) && @@ -2269,18 +2503,20 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set accel bias model values!!!\n"); - ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); - ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2], + ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]); + ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", + readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]); } res.success = true; return true; - } - //Set accel magnitude error adaptive measurement values - bool Microstrain::set_accel_adaptive_vals(microstrain_mips::SetAccelAdaptiveVals::Request &req, microstrain_mips::SetAccelAdaptiveVals::Response &res ) + // Set accel magnitude error adaptive measurement values + bool Microstrain::set_accel_adaptive_vals(microstrain_mips::SetAccelAdaptiveVals::Request &req, + microstrain_mips::SetAccelAdaptiveVals::Response &res ) { ROS_INFO("Setting the accel magnitude error adaptive measurement values\n"); @@ -2293,27 +2529,29 @@ namespace Microstrain accel_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK) + while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); break; } } - //Read back the accel magnitude error adaptive measurement values + // Read back the accel magnitude error adaptive measurement values start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); break; } } - if((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) && + if ((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) && (abs(accel_magnitude_error_command.low_pass_cutoff - accel_magnitude_error_readback.low_pass_cutoff) < 0.001) && (abs(accel_magnitude_error_command.min_1sigma - accel_magnitude_error_readback.min_1sigma) < 0.001) && (abs(accel_magnitude_error_command.low_limit - accel_magnitude_error_readback.low_limit) < 0.001) && @@ -2326,37 +2564,50 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set accel magnitude error adaptive measurement values!!!"); - ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_command.enable, accel_magnitude_error_command.low_pass_cutoff, accel_magnitude_error_command.min_1sigma, accel_magnitude_error_command.low_limit, accel_magnitude_error_command.high_limit, accel_magnitude_error_command.low_limit_1sigma, accel_magnitude_error_command.high_limit_1sigma); - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f", + accel_magnitude_error_command.enable, accel_magnitude_error_command.low_pass_cutoff, + accel_magnitude_error_command.min_1sigma, accel_magnitude_error_command.low_limit, + accel_magnitude_error_command.high_limit, accel_magnitude_error_command.low_limit_1sigma, + accel_magnitude_error_command.high_limit_1sigma); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f", + accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, + accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, + accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, + accel_magnitude_error_readback.high_limit_1sigma); } res.success = true; return true; - } - //Get accep magnitude error adaptive measurement values + // Get accep magnitude error adaptive measurement values bool Microstrain::get_accel_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) { start = clock(); - while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out."); break; } } - ROS_INFO("Accel magnitude error adaptive measurement values are: Enable: %i, Parameters: %f %f %f %f %f %f", accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, accel_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("Accel magnitude error adaptive measurement values are: Enable: %i, Parameters: %f %f %f %f %f %f", + accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff, + accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit, + accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma, + accel_magnitude_error_readback.high_limit_1sigma); res.success = true; return true; } - //Set magnetometer magnitude error adaptive measurement values - bool Microstrain::set_mag_adaptive_vals(microstrain_mips::SetMagAdaptiveVals::Request &req, microstrain_mips::SetMagAdaptiveVals::Response &res ) + // Set magnetometer magnitude error adaptive measurement values + bool Microstrain::set_mag_adaptive_vals(microstrain_mips::SetMagAdaptiveVals::Request &req, + microstrain_mips::SetMagAdaptiveVals::Response &res ) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2374,27 +2625,29 @@ namespace Microstrain mag_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma; start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_magnitude_error_command) != MIP_INTERFACE_OK) + while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, + &mag_magnitude_error_command) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; } } - //Read back the mag magnitude error adaptive measurement values + // Read back the mag magnitude error adaptive measurement values start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; } } - if((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) && + if ((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) && (abs(mag_magnitude_error_command.low_pass_cutoff - mag_magnitude_error_readback.low_pass_cutoff) < 0.001) && (abs(mag_magnitude_error_command.min_1sigma - mag_magnitude_error_readback.min_1sigma) < 0.001) && (abs(mag_magnitude_error_command.low_limit - mag_magnitude_error_readback.low_limit) < 0.001) && @@ -2407,18 +2660,26 @@ namespace Microstrain else { ROS_INFO("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n"); - ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_command.enable, mag_magnitude_error_command.low_pass_cutoff, mag_magnitude_error_command.min_1sigma, mag_magnitude_error_command.low_limit, mag_magnitude_error_command.high_limit, mag_magnitude_error_command.low_limit_1sigma, mag_magnitude_error_command.high_limit_1sigma); - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", + mag_magnitude_error_command.enable, mag_magnitude_error_command.low_pass_cutoff, + mag_magnitude_error_command.min_1sigma, mag_magnitude_error_command.low_limit, + mag_magnitude_error_command.high_limit, mag_magnitude_error_command.low_limit_1sigma, + mag_magnitude_error_command.high_limit_1sigma); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", + mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, + mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, + mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, + mag_magnitude_error_readback.high_limit_1sigma); } res.success = true; return true; } - //Get magnetometer magnitude error adaptive measurement values + // Get magnetometer magnitude error adaptive measurement values bool Microstrain::get_mag_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) { - if(GX5_15 == true) + if (GX5_15 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2426,24 +2687,29 @@ namespace Microstrain } start = clock(); - while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; } } - ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, mag_magnitude_error_readback.high_limit_1sigma); + ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", + mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff, + mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit, + mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma, + mag_magnitude_error_readback.high_limit_1sigma); res.success = true; return true; } - //Get magnetometer dip angle error adaptive measurement values + // Get magnetometer dip angle error adaptive measurement values bool Microstrain::get_mag_dip_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res ) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2451,9 +2717,10 @@ namespace Microstrain } start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; @@ -2466,14 +2733,15 @@ namespace Microstrain mag_dip_angle_error_readback.high_limit, mag_dip_angle_error_readback.high_limit_1sigma); - res.success = true; - return true; + res.success = true; + return true; } - //Get magnetometer dip angle error adaptive measurement values - bool Microstrain::set_mag_dip_adaptive_vals(microstrain_mips::SetMagDipAdaptiveVals::Request &req, microstrain_mips::SetMagDipAdaptiveVals::Response &res ) + // Get magnetometer dip angle error adaptive measurement values + bool Microstrain::set_mag_dip_adaptive_vals(microstrain_mips::SetMagDipAdaptiveVals::Request &req, + microstrain_mips::SetMagDipAdaptiveVals::Response &res ) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2489,27 +2757,29 @@ namespace Microstrain mag_dip_angle_error_command.high_limit_1sigma = req.high_limit_1sigma; start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK) + while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; } } - //Read back the mag magnitude error adaptive measurement values + // Read back the mag magnitude error adaptive measurement values start = clock(); - while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) + while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out."); break; } } - if((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) && + if ((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) && (abs(mag_dip_angle_error_command.low_pass_cutoff - mag_dip_angle_error_readback.low_pass_cutoff) < 0.001) && (abs(mag_dip_angle_error_command.min_1sigma - mag_dip_angle_error_readback.min_1sigma) < 0.001) && (abs(mag_dip_angle_error_command.high_limit - mag_dip_angle_error_readback.high_limit) < 0.001) && @@ -2537,10 +2807,11 @@ namespace Microstrain return true; } - //Get vehicle dynamics mode - bool Microstrain::get_dynamics_mode(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) + // Get vehicle dynamics mode + bool Microstrain::get_dynamics_mode(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2548,7 +2819,8 @@ namespace Microstrain } readback_dynamics_mode = 0; - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) {} ROS_INFO("Vehicle dynamics mode is: %d\n", dynamics_mode); @@ -2557,10 +2829,11 @@ namespace Microstrain } - //Set vehicle dynamics mode. Only in 45 model. - bool Microstrain::set_dynamics_mode(microstrain_mips::SetDynamicsMode::Request &req, microstrain_mips::SetDynamicsMode::Response &res) + // Set vehicle dynamics mode. Only in 45 model. + bool Microstrain::set_dynamics_mode(microstrain_mips::SetDynamicsMode::Request &req, + microstrain_mips::SetDynamicsMode::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2577,9 +2850,10 @@ namespace Microstrain else { start = clock(); - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out."); break; @@ -2587,9 +2861,10 @@ namespace Microstrain } readback_dynamics_mode = 0; - while(mip_filter_vehicle_dynamics_mode(&device_interface_, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){} + while (mip_filter_vehicle_dynamics_mode(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) {} - if(dynamics_mode == readback_dynamics_mode) + if (dynamics_mode == readback_dynamics_mode) { ROS_INFO("Vehicle dynamics mode successfully set to %d\n", dynamics_mode); res.success = true; @@ -2603,10 +2878,11 @@ namespace Microstrain return true; } - //Set sensor to vehicle frame offset. Only in 45 - bool Microstrain::set_sensor_vehicle_frame_offset(microstrain_mips::SetSensorVehicleFrameOffset::Request &req, microstrain_mips::SetSensorVehicleFrameOffset::Response &res) + // Set sensor to vehicle frame offset. Only in 45 + bool Microstrain::set_sensor_vehicle_frame_offset(microstrain_mips::SetSensorVehicleFrameOffset::Request &req, + microstrain_mips::SetSensorVehicleFrameOffset::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2622,27 +2898,29 @@ namespace Microstrain offset[2] = req.offset.z; start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_offset(&device_interface_, + MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); break; } } - //Read back the transformation + // Read back the transformation start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_offset(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); break; } } - if((abs(readback_offset[0]-offset[0]) < 0.001) && + if ((abs(readback_offset[0]-offset[0]) < 0.001) && (abs(readback_offset[1]-offset[1]) < 0.001) && (abs(readback_offset[2]-offset[2]) < 0.001)) { @@ -2659,10 +2937,10 @@ namespace Microstrain return true; } - //Get sensor to vehicle frame offset. Only in 45 model. + // Get sensor to vehicle frame offset. Only in 45 model. bool Microstrain::get_sensor_vehicle_frame_offset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - if(GX5_15 == true || GX5_25 == true) + if (GX5_15 == true || GX5_25 == true) { ROS_INFO("Device does not support this feature"); res.success = false; @@ -2672,9 +2950,10 @@ namespace Microstrain memset(readback_offset, 0, 3*sizeof(float)); start = clock(); - while(mip_filter_sensor2vehicle_offset(&device_interface_, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) + while (mip_filter_sensor2vehicle_offset(&device_interface_, + MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_filter_sensor2vehicle_offset function timed out."); break; @@ -2687,34 +2966,39 @@ namespace Microstrain return true; } - //Get basic or diagnostic status of device. Called by basic and diagnostic services. - u16 Microstrain::mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer) + // Get basic or diagnostic status of device. Called by basic and diagnostic services. + u16 Microstrain::mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, + u16 model_number, u8 status_selector, u8 *response_buffer) { int total_size = 0; - if(GX5_25) + if (GX5_25) { gx4_25_basic_status_field *basic_ptr; gx4_25_diagnostic_device_status_field *diagnostic_ptr; u16 response_size = MIP_FIELD_HEADER_SIZE; - //Set response size based on device model and whether basic or diagnostic status is chosen - if(status_selector == GX4_25_BASIC_STATUS_SEL) + // Set response size based on device model and whether basic or diagnostic status is chosen + if (status_selector == GX4_25_BASIC_STATUS_SEL) + { response_size += sizeof(gx4_25_basic_status_field); - else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) + } + else if (status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) + { response_size += sizeof(gx4_25_diagnostic_device_status_field); + } - while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){} + while (mip_3dm_cmd_device_status(device_interface, model_number, + status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK) {} - if(status_selector == GX4_25_BASIC_STATUS_SEL) + if (status_selector == GX4_25_BASIC_STATUS_SEL) { - if(response_size != sizeof(gx4_25_basic_status_field)) + if (response_size != sizeof(gx4_25_basic_status_field)) { return MIP_INTERFACE_ERROR; } - else if(MIP_SDK_CONFIG_BYTESWAP) + else if (MIP_SDK_CONFIG_BYTESWAP) { - - //Perform byteswapping + // Perform byteswapping byteswap_inplace(&response_buffer[0], sizeof(basic_field.device_model)); byteswap_inplace(&response_buffer[2], sizeof(basic_field.status_selector)); byteswap_inplace(&response_buffer[3], sizeof(basic_field.status_flags)); @@ -2725,95 +3009,123 @@ namespace Microstrain void * struct_pointer; struct_pointer = &basic_field; - //Copy response from response buffer to basic status struct + // Copy response from response buffer to basic status struct memcpy(struct_pointer, response_buffer, sizeof(basic_field.device_model)); memcpy((struct_pointer+2), &(response_buffer[2]), sizeof(basic_field.status_selector)); memcpy((struct_pointer+3), &(response_buffer[3]), sizeof(basic_field.status_flags)); memcpy((struct_pointer+7), &(response_buffer[7]), sizeof(basic_field.system_state)); memcpy((struct_pointer+9), &(response_buffer[9]), sizeof(basic_field.system_timer_ms)); - } - else if(status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) + else if (status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL) { - - if(response_size != sizeof(gx4_25_diagnostic_device_status_field)) + if (response_size != sizeof(gx4_25_diagnostic_device_status_field)) { return MIP_INTERFACE_ERROR; } - else if(MIP_SDK_CONFIG_BYTESWAP) + else if (MIP_SDK_CONFIG_BYTESWAP) { - - //byteswap and copy response to diagnostic status struct - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.device_model)); + // byteswap and copy response to diagnostic status struct + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.device_model)); total_size += sizeof(diagnostic_field.device_model); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_selector)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.status_selector)); total_size += sizeof(diagnostic_field.status_selector); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.status_flags)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.status_flags)); total_size += sizeof(diagnostic_field.status_flags); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_state)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.system_state)); total_size += sizeof(diagnostic_field.system_state); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.system_timer_ms)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.system_timer_ms)); total_size += sizeof(diagnostic_field.system_timer_ms); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_stream_enabled)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.imu_stream_enabled)); total_size += sizeof(diagnostic_field.imu_stream_enabled); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_stream_enabled)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.filter_stream_enabled)); total_size += sizeof(diagnostic_field.filter_stream_enabled); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_dropped_packets)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.imu_dropped_packets)); total_size += sizeof(diagnostic_field.imu_dropped_packets); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.filter_dropped_packets)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.filter_dropped_packets)); total_size += sizeof(diagnostic_field.filter_dropped_packets); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_written)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.com1_port_bytes_written)); total_size += sizeof(diagnostic_field.com1_port_bytes_written); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_bytes_read)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.com1_port_bytes_read)); total_size += sizeof(diagnostic_field.com1_port_bytes_read); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_write_overruns)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.com1_port_write_overruns)); total_size += sizeof(diagnostic_field.com1_port_write_overruns); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.com1_port_read_overruns)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.com1_port_read_overruns)); total_size += sizeof(diagnostic_field.com1_port_read_overruns); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_parser_errors)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.imu_parser_errors)); total_size += sizeof(diagnostic_field.imu_parser_errors); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_message_count)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.imu_message_count)); total_size += sizeof(diagnostic_field.imu_message_count); - byteswap_inplace(&response_buffer[total_size], sizeof(diagnostic_field.imu_last_message_ms)); + byteswap_inplace(&response_buffer[total_size], + sizeof(diagnostic_field.imu_last_message_ms)); } void * struct_pointer; struct_pointer = &diagnostic_field; total_size = 0; - memcpy(struct_pointer, response_buffer, sizeof(diagnostic_field.device_model)); + memcpy(struct_pointer, response_buffer, + sizeof(diagnostic_field.device_model)); total_size += sizeof(diagnostic_field.device_model); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_selector)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.status_selector)); total_size += sizeof(diagnostic_field.status_selector); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.status_flags)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.status_flags)); total_size += sizeof(diagnostic_field.status_flags); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_state)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.system_state)); total_size += sizeof(diagnostic_field.system_state); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.system_timer_ms)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.system_timer_ms)); total_size += sizeof(diagnostic_field.system_timer_ms); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_stream_enabled)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.imu_stream_enabled)); total_size += sizeof(diagnostic_field.imu_stream_enabled); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_stream_enabled)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.filter_stream_enabled)); total_size += sizeof(diagnostic_field.filter_stream_enabled); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_dropped_packets)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.imu_dropped_packets)); total_size += sizeof(diagnostic_field.imu_dropped_packets); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.filter_dropped_packets)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.filter_dropped_packets)); total_size += sizeof(diagnostic_field.filter_dropped_packets); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_written)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.com1_port_bytes_written)); total_size += sizeof(diagnostic_field.com1_port_bytes_written); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_bytes_read)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.com1_port_bytes_read)); total_size += sizeof(diagnostic_field.com1_port_bytes_read); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_write_overruns)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.com1_port_write_overruns)); total_size += sizeof(diagnostic_field.com1_port_write_overruns); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.com1_port_read_overruns)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.com1_port_read_overruns)); total_size += sizeof(diagnostic_field.com1_port_read_overruns); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_parser_errors)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.imu_parser_errors)); total_size += sizeof(diagnostic_field.imu_parser_errors); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_message_count)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.imu_message_count)); total_size += sizeof(diagnostic_field.imu_message_count); - memcpy((struct_pointer + total_size), &(response_buffer[total_size]), sizeof(diagnostic_field.imu_last_message_ms)); + memcpy((struct_pointer + total_size), &(response_buffer[total_size]), + sizeof(diagnostic_field.imu_last_message_ms)); total_size += sizeof(diagnostic_field.imu_last_message_ms); - } else return MIP_INTERFACE_ERROR; @@ -2822,7 +3134,7 @@ namespace Microstrain } } - //Start callbacks for data packets + // Start callbacks for data packets //////////////////////////////////////////////////////////////////////////////// // @@ -2839,29 +3151,28 @@ namespace Microstrain if (!publish_odom_) return; - //ROS_INFO("Filter callback"); - //The packet callback can have several types, process them all - switch(callback_type) + // ROS_INFO("Filter callback"); + // The packet callback can have several types, process them all + switch (callback_type) { /// - //Handle valid packets + // Handle valid packets /// case MIP_INTERFACE_CALLBACK_VALID_PACKET: { filter_valid_packet_count_++; /// - //Loop through all of the data fields + // Loop through all of the data fields /// - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) { - /// // Decode the field /// - switch(field_header->descriptor) + switch (field_header->descriptor) { /// // Estimated LLH Position @@ -2871,7 +3182,7 @@ namespace Microstrain { memcpy(&curr_filter_pos_, field_data, sizeof(mip_filter_llh_pos)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_filter_llh_pos_byteswap(&curr_filter_pos_); nav_msg_.header.seq = filter_valid_packet_count_; @@ -2881,8 +3192,8 @@ namespace Microstrain nav_msg_.pose.pose.position.y = curr_filter_pos_.latitude; nav_msg_.pose.pose.position.x = curr_filter_pos_.longitude; nav_msg_.pose.pose.position.z = curr_filter_pos_.ellipsoid_height; - - }break; + } + break; /// // Estimated NED Velocity @@ -2892,7 +3203,7 @@ namespace Microstrain { memcpy(&curr_filter_vel_, field_data, sizeof(mip_filter_ned_velocity)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_filter_ned_velocity_byteswap(&curr_filter_vel_); // rotate velocities from NED to sensor coordinates @@ -2905,12 +3216,13 @@ namespace Microstrain tf2::Vector3 vel_enu(curr_filter_vel_.east, curr_filter_vel_.north, -1.0*curr_filter_vel_.down); - tf2::Vector3 vel_in_sensor_frame = tf2::quatRotate(nav_quat.inverse(),vel_enu); + tf2::Vector3 vel_in_sensor_frame = tf2::quatRotate(nav_quat.inverse(), vel_enu); - nav_msg_.twist.twist.linear.x = vel_in_sensor_frame[0]; //curr_filter_vel_.east; - nav_msg_.twist.twist.linear.y = vel_in_sensor_frame[1]; //curr_filter_vel_.north; - nav_msg_.twist.twist.linear.z = vel_in_sensor_frame[2]; //-1*curr_filter_vel_.down; - }break; + nav_msg_.twist.twist.linear.x = vel_in_sensor_frame[0]; // curr_filter_vel_.east; + nav_msg_.twist.twist.linear.y = vel_in_sensor_frame[1]; // curr_filter_vel_.north; + nav_msg_.twist.twist.linear.z = vel_in_sensor_frame[2]; // -1*curr_filter_vel_.down; + } + break; /// // Estimated Attitude, Euler Angles @@ -2920,16 +3232,17 @@ namespace Microstrain { memcpy(&curr_filter_angles_, field_data, sizeof(mip_filter_attitude_euler_angles)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_); - }break; + } + 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 + // For little-endian targets, byteswap the data field mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_); // put into ENU - swap X/Y, invert Z @@ -2937,75 +3250,75 @@ namespace Microstrain nav_msg_.pose.pose.orientation.y = curr_filter_quaternion_.q[1]; nav_msg_.pose.pose.orientation.z = -1.0*curr_filter_quaternion_.q[3]; nav_msg_.pose.pose.orientation.w = curr_filter_quaternion_.q[0]; - - }break; + } + 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 + // For little-endian targets, byteswap the data field mip_filter_compensated_angular_rate_byteswap(&curr_filter_angular_rate_); nav_msg_.twist.twist.angular.x = curr_filter_angular_rate_.x; nav_msg_.twist.twist.angular.y = curr_filter_angular_rate_.y; nav_msg_.twist.twist.angular.z = curr_filter_angular_rate_.z; - - - }break; + } + break; // Position Uncertainty case MIP_FILTER_DATA_POS_UNCERTAINTY: { memcpy(&curr_filter_pos_uncertainty_, field_data, sizeof(mip_filter_llh_pos_uncertainty)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_filter_llh_pos_uncertainty_byteswap(&curr_filter_pos_uncertainty_); - //x-axis + // x-axis nav_msg_.pose.covariance[0] = curr_filter_pos_uncertainty_.east*curr_filter_pos_uncertainty_.east; nav_msg_.pose.covariance[7] = curr_filter_pos_uncertainty_.north*curr_filter_pos_uncertainty_.north; nav_msg_.pose.covariance[14] = curr_filter_pos_uncertainty_.down*curr_filter_pos_uncertainty_.down; - }break; + } + break; // Velocity Uncertainty case MIP_FILTER_DATA_VEL_UNCERTAINTY: { memcpy(&curr_filter_vel_uncertainty_, field_data, sizeof(mip_filter_ned_vel_uncertainty)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_filter_ned_vel_uncertainty_byteswap(&curr_filter_vel_uncertainty_); nav_msg_.twist.covariance[0] = curr_filter_vel_uncertainty_.east*curr_filter_vel_uncertainty_.east; nav_msg_.twist.covariance[7] = curr_filter_vel_uncertainty_.north*curr_filter_vel_uncertainty_.north; nav_msg_.twist.covariance[14] = curr_filter_vel_uncertainty_.down*curr_filter_vel_uncertainty_.down; - - }break; + } + break; // Attitude Uncertainty 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 + // For little-endian targets, byteswap the data field mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_); nav_msg_.pose.covariance[21] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll; nav_msg_.pose.covariance[28] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch; nav_msg_.pose.covariance[35] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw; - - }break; + } + 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 + // For little-endian targets, byteswap the data field mip_filter_status_byteswap(&curr_filter_status_); nav_status_msg_.data.clear(); - ROS_DEBUG_THROTTLE(1.0,"Filter Status: %#06X, Dyn. Mode: %#06X, Filter State: %#06X", + 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); @@ -3013,9 +3326,8 @@ namespace Microstrain nav_status_msg_.data.push_back(curr_filter_status_.dynamics_mode); nav_status_msg_.data.push_back(curr_filter_status_.status_flags); nav_status_pub_.publish(nav_status_msg_); - - - }break; + } + break; default: break; } @@ -3023,52 +3335,57 @@ namespace Microstrain // Publish nav_pub_.publish(nav_msg_); - }break; + } + break; /// - //Handle checksum error packets + // Handle checksum error packets /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: { filter_checksum_error_packet_count_++; - }break; + } + break; /// - //Handle timeout packets + // Handle timeout packets /// case MIP_INTERFACE_CALLBACK_TIMEOUT: { filter_timeout_packet_count_++; - }break; + } + break; default: break; } print_packet_stats(); - } // filter_packet_callback + } // filter_packet_callback - //Send diagnostic information to device status topic and diagnostic aggregator + // Send diagnostic information to device status topic and diagnostic aggregator void Microstrain::device_status_callback() { - if(GX5_25) + if (GX5_25) { u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)]; start = clock(); - while(mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK) + while (mip_3dm_cmd_hw_specific_device_status(&device_interface_, + GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL, + response_buffer) != MIP_INTERFACE_OK) { - if(clock() - start > 5000) + if (clock() - start > 5000) { ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out."); break; } } - //ROS_INFO("Adding device diagnostics to status msg"); - //ROS_INFO("adding data to message"); + // ROS_INFO("Adding device diagnostics to status msg"); + // ROS_INFO("adding data to message"); device_status_msg_.device_model = diagnostic_field.device_model; device_status_msg_.status_selector = diagnostic_field.status_selector; device_status_msg_.status_flags = diagnostic_field.status_flags; @@ -3108,11 +3425,11 @@ namespace Microstrain // If we aren't publishing, then return if (!publish_imu_) return; - //The packet callback can have several types, process them all - switch(callback_type) + // The packet callback can have several types, process them all + switch (callback_type) { /// - //Handle valid packets + // Handle valid packets /// case MIP_INTERFACE_CALLBACK_VALID_PACKET: @@ -3120,17 +3437,16 @@ namespace Microstrain ahrs_valid_packet_count_++; /// - //Loop through all of the data fields + // Loop through all of the data fields /// - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) { - /// // Decode the field /// - switch(field_header->descriptor) + switch (field_header->descriptor) { /// // Scaled Accelerometer @@ -3140,7 +3456,7 @@ namespace Microstrain { memcpy(&curr_ahrs_accel_, field_data, sizeof(mip_ahrs_scaled_accel)); - //For little-endian targets, byteswap the data field + // 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 @@ -3151,11 +3467,12 @@ namespace Microstrain imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0]; imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1]; imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2]; - // Since the sensor does not produce a covariance for linear acceleration, set it based - // on our pulled in parameters. - std::copy( imu_linear_cov_.begin(), imu_linear_cov_.end(), imu_msg_.linear_acceleration_covariance.begin()); - - }break; + // Since the sensor does not produce a covariance for linear acceleration, + // set it based on our pulled in parameters. + std::copy(imu_linear_cov_.begin(), imu_linear_cov_.end(), + imu_msg_.linear_acceleration_covariance.begin()); + } + break; /// // Scaled Gyro @@ -3165,7 +3482,7 @@ namespace Microstrain { memcpy(&curr_ahrs_gyro_, field_data, sizeof(mip_ahrs_scaled_gyro)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro_); imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0]; @@ -3173,9 +3490,10 @@ namespace Microstrain imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2]; // Since the sensor does not produce a covariance for angular velocity, set it based // on our pulled in parameters. - std::copy( imu_angular_cov_.begin(), imu_angular_cov_.end(), imu_msg_.angular_velocity_covariance.begin()); - - }break; + std::copy(imu_angular_cov_.begin(), imu_angular_cov_.end(), + imu_msg_.angular_velocity_covariance.begin()); + } + break; /// // Scaled Magnetometer @@ -3185,27 +3503,29 @@ namespace Microstrain { memcpy(&curr_ahrs_mag_, field_data, sizeof(mip_ahrs_scaled_mag)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag_); - - }break; + } + 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 + // For little-endian targets, byteswap the data field mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_); // put into ENU - swap X/Y, invert Z imu_msg_.orientation.x = curr_ahrs_quaternion_.q[2]; imu_msg_.orientation.y = curr_ahrs_quaternion_.q[1]; imu_msg_.orientation.z = -1.0*curr_ahrs_quaternion_.q[3]; imu_msg_.orientation.w = curr_ahrs_quaternion_.q[0]; - // Since the MIP_AHRS data does not contain uncertainty values we have to set them based on the parameter values. - std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(), imu_msg_.orientation_covariance.begin()); - - }break; + // Since the MIP_AHRS data does not contain uncertainty values + // we have to set them based on the parameter values. + std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(), + imu_msg_.orientation_covariance.begin()); + } + break; default: break; } @@ -3213,31 +3533,33 @@ namespace Microstrain // Publish imu_pub_.publish(imu_msg_); - - }break; + } + break; /// - //Handle checksum error packets + // Handle checksum error packets /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: { ahrs_checksum_error_packet_count_++; - }break; + } + break; /// - //Handle timeout packets + // Handle timeout packets /// case MIP_INTERFACE_CALLBACK_TIMEOUT: { ahrs_timeout_packet_count_++; - }break; + } + break; default: break; } print_packet_stats(); - } // ahrs_packet_callback + } // ahrs_packet_callback //////////////////////////////////////////////////////////////////////////////// @@ -3255,11 +3577,11 @@ namespace Microstrain // If we aren't publishing, then return if (!publish_gps_) return; - //The packet callback can have several types, process them all - switch(callback_type) + // The packet callback can have several types, process them all + switch (callback_type) { /// - //Handle valid packets + // Handle valid packets /// case MIP_INTERFACE_CALLBACK_VALID_PACKET: @@ -3267,17 +3589,16 @@ namespace Microstrain gps_valid_packet_count_++; /// - //Loop through all of the data fields + // Loop through all of the data fields /// - while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) + while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK) { - /// // Decode the field /// - switch(field_header->descriptor) + switch (field_header->descriptor) { /// // LLH Position @@ -3287,7 +3608,7 @@ namespace Microstrain { memcpy(&curr_llh_pos_, field_data, sizeof(mip_gps_llh_pos)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_gps_llh_pos_byteswap(&curr_llh_pos_); // push into ROS message @@ -3304,8 +3625,8 @@ namespace Microstrain gps_msg_.header.seq = gps_valid_packet_count_; gps_msg_.header.stamp = ros::Time::now(); gps_msg_.header.frame_id = gps_frame_id_; - - }break; + } + break; /// // NED Velocity @@ -3315,10 +3636,10 @@ namespace Microstrain { memcpy(&curr_ned_vel_, field_data, sizeof(mip_gps_ned_vel)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_gps_ned_vel_byteswap(&curr_ned_vel_); - - }break; + } + break; /// // GPS Time @@ -3328,35 +3649,38 @@ namespace Microstrain { memcpy(&curr_gps_time_, field_data, sizeof(mip_gps_time)); - //For little-endian targets, byteswap the data field + // For little-endian targets, byteswap the data field mip_gps_time_byteswap(&curr_gps_time_); - - }break; + } + break; default: break; } } - }break; + } + break; /// - //Handle checksum error packets + // Handle checksum error packets /// case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR: { msgvalid = 0; gps_checksum_error_packet_count_++; - }break; + } + break; /// - //Handle timeout packets + // Handle timeout packets /// case MIP_INTERFACE_CALLBACK_TIMEOUT: { msgvalid = 0; gps_timeout_packet_count_++; - }break; + } + break; default: break; } @@ -3368,14 +3692,15 @@ namespace Microstrain } print_packet_stats(); - } // gps_packet_callback + } // gps_packet_callback void Microstrain::print_packet_stats() { - ROS_DEBUG_THROTTLE(1.0,"%u FILTER (%u errors) %u AHRS (%u errors) %u GPS (%u errors) Packets", filter_valid_packet_count_, filter_timeout_packet_count_ + filter_checksum_error_packet_count_, + ROS_DEBUG_THROTTLE(1.0, "%u FILTER (%u errors) %u AHRS (%u errors) %u GPS (%u errors) Packets", + 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_, gps_valid_packet_count_, gps_timeout_packet_count_ + gps_checksum_error_packet_count_); - } // print_packet_stats + } // print_packet_stats @@ -3383,20 +3708,20 @@ namespace Microstrain // Wrapper callbacks void filter_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type) { - Microstrain* ustrain = (Microstrain*) user_ptr; - ustrain->filter_packet_callback(user_ptr,packet,packet_size,callback_type); + Microstrain* ustrain = reinterpret_cast(user_ptr); + ustrain->filter_packet_callback(user_ptr, packet, packet_size, callback_type); } void ahrs_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type) { - Microstrain* ustrain = (Microstrain*) user_ptr; - ustrain->ahrs_packet_callback(user_ptr,packet,packet_size,callback_type); + Microstrain* ustrain = reinterpret_cast(user_ptr); + ustrain->ahrs_packet_callback(user_ptr, packet, packet_size, callback_type); } // Wrapper callbacks void gps_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type) { - Microstrain* ustrain = (Microstrain*) user_ptr; - ustrain->gps_packet_callback(user_ptr,packet,packet_size,callback_type); + Microstrain* ustrain = reinterpret_cast(user_ptr); + ustrain->gps_packet_callback(user_ptr, packet, packet_size, callback_type); } -} // Microstrain namespace +} // namespace Microstrain