microstrain_3dm.cpp
Go to the documentation of this file.
1 /*
2 
3 Copyright (c) 2017, Brian Bingham
4 
5 This code is licensed under MIT license (see LICENSE file for details)
6 
7 */
8 
9 #include <tf2/LinearMath/Transform.h>
10 #include <string>
11 #include <algorithm>
12 #include <time.h>
13 
14 #include "microstrain_mips/status_msg.h"
15 #include "microstrain_diagnostic_updater.h"
16 #include <vector>
17 
18 namespace Microstrain
19 {
20  Microstrain::Microstrain():
21  // Initialization list
22  filter_valid_packet_count_(0),
23  ahrs_valid_packet_count_(0),
24  gps_valid_packet_count_(0),
25  filter_timeout_packet_count_(0),
26  ahrs_timeout_packet_count_(0),
27  gps_timeout_packet_count_(0),
28  filter_checksum_error_packet_count_(0),
29  ahrs_checksum_error_packet_count_(0),
30  gps_checksum_error_packet_count_(0),
31  gps_frame_id_("gps_frame"),
32  imu_frame_id_("imu_frame"),
33  odom_frame_id_("odom_frame"),
34  odom_child_frame_id_("odom_frame"),
35  publish_gps_(true),
36  publish_imu_(true),
37  publish_odom_(true),
38  publish_filtered_imu_(false),
39  remove_imu_gravity_(false),
40  frame_based_enu_(false),
41  imu_linear_cov_(std::vector<double>(9, 0.0)),
42  imu_angular_cov_(std::vector<double>(9, 0.0)),
43  imu_orientation_cov_(std::vector<double>(9, 0.0))
44  {
45  // pass
46  }
47  Microstrain::~Microstrain()
48  {
49  // pass
50  }
51  void Microstrain::run()
52  {
53  // Variables for device configuration, ROS parameters, etc.
54  u32 com_port, baudrate;
55  bool device_setup = false;
56  bool readback_settings = true;
57  bool save_settings = true;
58  bool auto_init = true;
59  u8 auto_init_u8 = 1;
60  u8 readback_headingsource = 0;
61  u8 readback_auto_init = 0;
62  int declination_source;
63  u8 declination_source_u8;
64  u8 readback_declination_source;
65  double declination;
66 
67  // Variables
68  base_device_info_field device_info;
69  u8 enable = 1;
70  u8 data_stream_format_descriptors[10];
71  u16 data_stream_format_decimation[10];
72  u8 data_stream_format_num_entries = 0;
73  u8 readback_data_stream_format_descriptors[10] = {0};
74  u16 readback_data_stream_format_decimation[10] = {0};
75  u8 readback_data_stream_format_num_entries = 0;
76  u16 base_rate = 0;
77  u16 device_descriptors[128] = {0};
78  u16 device_descriptors_size = 128*2;
79  u8 gps_source = 0;
80  u8 heading_source = 0x1;
81  mip_low_pass_filter_settings filter_settings;
82  u16 duration = 0;
83  mip_filter_external_gps_update_command external_gps_update;
84  mip_filter_external_heading_update_command external_heading_update;
85  mip_filter_external_heading_with_time_command external_heading_with_time;
86 
87  com_mode = 0;
88 
89  // Device model flags
90  GX5_15 = false;
91  GX5_25 = false;
92  GX5_35 = false;
93  GX5_45 = false;
94 
95  // ROS setup
96  ros::Time::init();
97  ros::NodeHandle node;
98  ros::NodeHandle private_nh("~");
99 
100  // ROS Parameters
101  // Comms Parameters
102  std::string port;
103  int baud, pdyn_mode;
104  private_nh.param("port", port, std::string("/dev/ttyACM1"));
105  private_nh.param("baudrate", baud, 115200);
106  baudrate = (u32)baud;
107  // Configuration Parameters
108  private_nh.param("device_setup", device_setup, false);
109  private_nh.param("readback_settings", readback_settings, true);
110  private_nh.param("save_settings", save_settings, true);
111 
112  private_nh.param("auto_init", auto_init, true);
113  private_nh.param("gps_rate", gps_rate_, 1);
114  private_nh.param("imu_rate", imu_rate_, 10);
115  private_nh.param("nav_rate", nav_rate_, 10);
116  private_nh.param("dynamics_mode", pdyn_mode, 1);
117 
118  dynamics_mode = (u8)pdyn_mode;
119  if (dynamics_mode < 1 || dynamics_mode > 3)
120  {
121  ROS_WARN("dynamics_mode can't be %#04X, must be 1, 2 or 3. Setting to 1.", dynamics_mode);
122  dynamics_mode = 1;
123  }
124 
125  private_nh.param("declination_source", declination_source, 2);
126  if (declination_source < 1 || declination_source > 3)
127  {
128  ROS_WARN("declination_source can't be %#04X, must be 1, 2 or 3. Setting to 2.", declination_source);
129  declination_source = 2;
130  }
131  declination_source_u8 = (u8)declination_source;
132 
133  // declination_source_command=(u8)declination_source;
134  private_nh.param("declination", declination, 0.23);
135  private_nh.param("gps_frame_id", gps_frame_id_, std::string("wgs84"));
136  private_nh.param("imu_frame_id", imu_frame_id_, std::string("base_link"));
137  private_nh.param("odom_frame_id", odom_frame_id_, std::string("wgs84"));
138  private_nh.param("odom_child_frame_id", odom_child_frame_id_, std::string("base_link"));
139 
140  private_nh.param("publish_imu", publish_imu_, true);
141  private_nh.param("publish_bias", publish_bias_, true);
142  private_nh.param("publish_filtered_imu", publish_filtered_imu_, false);
143  private_nh.param("remove_imu_gravity", remove_imu_gravity_, false);
144  private_nh.param("frame_based_enu", frame_based_enu_, false);
145 
146  // Covariance parameters to set the sensor_msg/IMU covariance values
147  std::vector<double> default_cov(9, 0.0);
148  private_nh.param("imu_orientation_cov", imu_orientation_cov_, default_cov);
149  private_nh.param("imu_linear_cov", imu_linear_cov_, default_cov);
150  private_nh.param("imu_angular_cov", imu_angular_cov_, default_cov);
151 
152  // ROS publishers and subscribers
153  if (publish_imu_)
154  imu_pub_ = node.advertise<sensor_msgs::Imu>("imu/data", 100);
155  if (publish_filtered_imu_)
156  filtered_imu_pub_ = node.advertise<sensor_msgs::Imu>("filtered/imu/data", 100);
157 
158  // Publishes device status
159  device_status_pub_ = node.advertise<microstrain_mips::status_msg>("device/status", 100);
160 
161  // Services to set/get device functions
162  ros::ServiceServer reset_filter = node.advertiseService("reset_kf",
163  &Microstrain::reset_callback, this);
164  ros::ServiceServer device_report_service = node.advertiseService("device_report",
165  &Microstrain::device_report, this);
166  ros::ServiceServer gyro_bias_capture_service = node.advertiseService("gyro_bias_capture",
167  &Microstrain::gyro_bias_capture, this);
168  ros::ServiceServer set_soft_iron_matrix_service = node.advertiseService("set_soft_iron_matrix",
169  &Microstrain::set_soft_iron_matrix, this);
170  ros::ServiceServer set_complementary_filter_service = node.advertiseService("set_complementary_filter",
171  &Microstrain::set_complementary_filter, this);
172  ros::ServiceServer set_filter_euler_service = node.advertiseService("set_filter_euler",
173  &Microstrain::set_filter_euler, this);
174  ros::ServiceServer set_filter_heading_service = node.advertiseService("set_filter_heading",
175  &Microstrain::set_filter_heading, this);
176  ros::ServiceServer set_accel_bias_model_service = node.advertiseService("set_accel_bias_model",
177  &Microstrain::set_accel_bias_model, this);
178  ros::ServiceServer set_accel_adaptive_vals_service = node.advertiseService("set_accel_adaptive_vals",
179  &Microstrain::set_accel_adaptive_vals, this);
180  ros::ServiceServer set_sensor_vehicle_frame_trans_service = node.advertiseService("set_sensor_vehicle_frame_trans",
181  &Microstrain::set_sensor_vehicle_frame_trans, this);
182  ros::ServiceServer set_sensor_vehicle_frame_offset_service = node.advertiseService("set_sensor_vehicle_frame_offset",
183  &Microstrain::set_sensor_vehicle_frame_offset, this);
184  ros::ServiceServer set_accel_bias_service = node.advertiseService("set_accel_bias",
185  &Microstrain::set_accel_bias, this);
186  ros::ServiceServer set_gyro_bias_service = node.advertiseService("set_gyro_bias",
187  &Microstrain::set_gyro_bias, this);
188  ros::ServiceServer set_hard_iron_values_service = node.advertiseService("set_hard_iron_values",
189  &Microstrain::set_hard_iron_values, this);
190  ros::ServiceServer get_accel_bias_service = node.advertiseService("get_accel_bias",
191  &Microstrain::get_accel_bias, this);
192  ros::ServiceServer get_gyro_bias_service = node.advertiseService("get_gyro_bias",
193  &Microstrain::get_gyro_bias, this);
194  ros::ServiceServer get_hard_iron_values_service = node.advertiseService("get_hard_iron_values",
195  &Microstrain::get_hard_iron_values, this);
196  ros::ServiceServer get_soft_iron_matrix_service = node.advertiseService("get_soft_iron_matrix",
197  &Microstrain::get_soft_iron_matrix, this);
198  ros::ServiceServer get_sensor_vehicle_frame_trans_service = node.advertiseService("get_sensor_vehicle_frame_trans",
199  &Microstrain::get_sensor_vehicle_frame_trans, this);
200  ros::ServiceServer get_complementary_filter_service = node.advertiseService("get_complementary_filter",
201  &Microstrain::get_complementary_filter, this);
202  ros::ServiceServer set_reference_position_service = node.advertiseService("set_reference_position",
203  &Microstrain::set_reference_position, this);
204  ros::ServiceServer get_reference_position_service = node.advertiseService("get_reference_position",
205  &Microstrain::get_reference_position, this);
206  ros::ServiceServer set_coning_sculling_comp_service = node.advertiseService("set_coning_sculling_comp",
207  &Microstrain::set_coning_sculling_comp, this);
208  ros::ServiceServer get_coning_sculling_comp_service = node.advertiseService("get_coning_sculling_comp",
209  &Microstrain::get_coning_sculling_comp, this);
210  ros::ServiceServer set_estimation_control_flags_service = node.advertiseService("set_estimation_control_flags",
211  &Microstrain::set_estimation_control_flags, this);
212  ros::ServiceServer get_estimation_control_flags_service = node.advertiseService("get_estimation_control_flags",
213  &Microstrain::get_estimation_control_flags, this);
214  ros::ServiceServer set_dynamics_mode_service = node.advertiseService("set_dynamics_mode",
215  &Microstrain::set_dynamics_mode, this);
216  ros::ServiceServer get_basic_status_service = node.advertiseService("get_basic_status",
217  &Microstrain::get_basic_status, this);
218  ros::ServiceServer get_diagnostic_report_service = node.advertiseService("get_diagnostic_report",
219  &Microstrain::get_diagnostic_report, this);
220  ros::ServiceServer set_zero_angle_update_threshold_service = node.advertiseService("set_zero_angle_update_threshold",
221  &Microstrain::set_zero_angle_update_threshold, this);
222  ros::ServiceServer get_zero_angle_update_threshold_service = node.advertiseService("get_zero_angle_update_threshold",
223  &Microstrain::get_zero_angle_update_threshold, this);
224  ros::ServiceServer set_tare_orientation_service = node.advertiseService("set_tare_orientation",
225  &Microstrain::set_tare_orientation, this);
226  ros::ServiceServer set_accel_noise_service = node.advertiseService("set_accel_noise",
227  &Microstrain::set_accel_noise, this);
228  ros::ServiceServer get_accel_noise_service = node.advertiseService("get_accel_noise",
229  &Microstrain::get_accel_noise, this);
230  ros::ServiceServer set_gyro_noise_service = node.advertiseService("set_gyro_noise",
231  &Microstrain::set_gyro_noise, this);
232  ros::ServiceServer get_gyro_noise_service = node.advertiseService("get_gyro_noise",
233  &Microstrain::get_gyro_noise, this);
234  ros::ServiceServer set_mag_noise_service = node.advertiseService("set_mag_noise",
235  &Microstrain::set_mag_noise, this);
236  ros::ServiceServer get_mag_noise_service = node.advertiseService("get_mag_noise",
237  &Microstrain::get_mag_noise, this);
238  ros::ServiceServer set_gyro_bias_model_service = node.advertiseService("set_gyro_bias_model",
239  &Microstrain::set_gyro_bias_model, this);
240  ros::ServiceServer get_gyro_bias_model_service = node.advertiseService("get_gyro_bias_model",
241  &Microstrain::get_gyro_bias_model, this);
242  ros::ServiceServer get_accel_adaptive_vals_service = node.advertiseService("get_accel_adaptive_vals",
243  &Microstrain::get_accel_adaptive_vals, this);
244  ros::ServiceServer set_mag_adaptive_vals_service = node.advertiseService("set_mag_adaptive_vals",
245  &Microstrain::set_mag_adaptive_vals, this);
246  ros::ServiceServer get_mag_adaptive_vals_service = node.advertiseService("get_mag_adaptive_vals",
247  &Microstrain::get_mag_adaptive_vals, this);
248  ros::ServiceServer set_mag_dip_adaptive_vals_service = node.advertiseService("set_mag_dip_adaptive_vals",
249  &Microstrain::set_mag_dip_adaptive_vals, this);
250  ros::ServiceServer get_accel_bias_model_service = node.advertiseService("get_accel_bias_model",
251  &Microstrain::get_accel_bias_model, this);
252  ros::ServiceServer get_mag_dip_adaptive_vals_service = node.advertiseService("get_mag_dip_adaptive_vals",
253  &Microstrain::get_mag_dip_adaptive_vals, this);
254  ros::ServiceServer get_sensor_vehicle_frame_offset_service = node.advertiseService("get_sensor_vehicle_frame_offset",
255  &Microstrain::get_sensor_vehicle_frame_offset, this);
256  ros::ServiceServer get_dynamics_mode_service = node.advertiseService("get_dynamics_mode",
257  &Microstrain::get_dynamics_mode, this);
258 
259  // Initialize the serial interface to the device
260  ROS_INFO("Attempting to open serial port <%s> at <%d> \n", port.c_str(), baudrate);
261  if (mip_interface_init(port.c_str(), baudrate,
262  &device_interface_, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK)
263  {
264  ROS_FATAL("Couldn't open serial port! Is it plugged in?");
265  }
266 
267  // We want to get the default device info even if we don't setup the device
268  // Get device info
269  start = clock();
270  while (mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK)
271  {
272  if (clock() - start > 5000)
273  {
274  ROS_INFO("mip_base_cmd_get_device_info function timed out.");
275  break;
276  }
277  }
278 
279  // Get device model name
280  memset(temp_string, 0, 20*sizeof(char));
281  memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
282  ROS_INFO("Model Name => %s\n", temp_string);
283  std::string model_name;
284 
285  for (int i = 6; i < 20; i++)
286  {
287  model_name += temp_string[i];
288  }
289 
290  // Set device model flag
291  model_name = model_name.c_str();
292  if (model_name == GX5_45_DEVICE)
293  {
294  GX5_45 = true;
295  }
296  if (model_name == GX5_35_DEVICE)
297  {
298  GX5_35 = true;
299  }
300  if (model_name == GX5_25_DEVICE)
301  {
302  GX5_25 = true;
303  }
304  if (model_name == GX5_15_DEVICE)
305  {
306  GX5_15 = true;
307  }
308 
310  // Device setup
311  float dT = 0.5; // common sleep time after setup communications
312  if (device_setup)
313  {
314  // Put device into standard mode - we never really use "direct mode"
315  ROS_INFO("Putting device communications into 'standard mode'");
316  device_descriptors_size = 128*2;
317  com_mode = MIP_SDK_GX4_45_IMU_STANDARD_MODE;
318  start = clock();
319  while (mip_system_com_mode(&device_interface_,
320  MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK)
321  {
322  if (clock() - start > 5000)
323  {
324  ROS_INFO("mip_system_com_mode function timed out.");
325  break;
326  }
327  }
328 
329  // Verify device mode setting
330  ROS_INFO("Verify comm's mode");
331  start = clock();
332  while (mip_system_com_mode(&device_interface_,
333  MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK)
334  {
335  if (clock() - start > 5000)
336  {
337  ROS_INFO("mip_system_com_mode function timed out.");
338  break;
339  }
340  }
341 
342  ROS_INFO("Sleep for a second...");
343  ros::Duration(dT).sleep();
344  ROS_INFO("Right mode?");
345  if (com_mode != MIP_SDK_GX4_45_IMU_STANDARD_MODE)
346  {
347  ROS_ERROR("Appears we didn't get into standard mode!");
348  }
349 
350  // Set GPS publishing to true if IMU model has GPS
351  if (GX5_45 || GX5_35)
352  {
353  private_nh.param("publish_gps", publish_gps_, true);
354  private_nh.param("publish_odom", publish_odom_, true);
355  }
356  else
357  {
358  private_nh.param("publish_gps", publish_gps_, false);
359  private_nh.param("publish_odom", publish_odom_, false);
360  }
361 
362  if (publish_gps_)
363  {
364  gps_pub_ = node.advertise<sensor_msgs::NavSatFix>("gps/fix", 100);
365  }
366 
367  if (publish_odom_)
368  {
369  nav_pub_ = node.advertise<nav_msgs::Odometry>("nav/odom", 100);
370  }
371 
372  // This is the EKF filter status, not just navigation/odom status
373  if (publish_odom_ || publish_filtered_imu_)
374  {
375  nav_status_pub_ = node.advertise<std_msgs::Int16MultiArray>("nav/status", 100);
376  }
377 
378  // Setup device callbacks
379  if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_FILTER_DATA_SET,
380  this, &filter_packet_callback_wrapper) != MIP_INTERFACE_OK)
381  {
382  ROS_FATAL("Can't setup filter callback!");
383  return;
384  }
385 
386  if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_AHRS_DATA_SET,
387  this, &ahrs_packet_callback_wrapper) != MIP_INTERFACE_OK)
388  {
389  ROS_FATAL("Can't setup ahrs callbacks!");
390  return;
391  }
392 
393  if (mip_interface_add_descriptor_set_callback(&device_interface_, MIP_GPS_DATA_SET,
394  this, &gps_packet_callback_wrapper) != MIP_INTERFACE_OK)
395  {
396  ROS_FATAL("Can't setup gpscallbacks!");
397  return;
398  }
399 
400  // Put into idle mode
401  ROS_INFO("Idling Device: Stopping data streams and/or waking from sleep");
402  start = clock();
403  while (mip_base_cmd_idle(&device_interface_) != MIP_INTERFACE_OK)
404  {
405  if (clock() - start > 5000)
406  {
407  ROS_INFO("mip_base_cmd_idle function timed out.");
408  break;
409  }
410  }
411  ros::Duration(dT).sleep();
412 
413  // AHRS Setup
414  // Get base rate
415  if (publish_imu_ || publish_filtered_imu_)
416  {
417  start = clock();
418  while (mip_3dm_cmd_get_ahrs_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK)
419  {
420  if (clock() - start > 5000)
421  {
422  ROS_INFO("mip_3dm_cmd_get_ahrs_base_rate function timed out.");
423  break;
424  }
425  }
426 
427  ROS_INFO("AHRS Base Rate => %d Hz", base_rate);
428  ros::Duration(dT).sleep();
429  // Deterimine decimation to get close to goal rate (We use the highest of the imu rates)
430  int rate = (imu_rate_ > nav_rate_) ? imu_rate_ : nav_rate_;
431  u8 imu_decimation = (u8)(static_cast<float>(base_rate)/ static_cast<float>(rate));
432  ROS_INFO("AHRS decimation set to %#04X", imu_decimation);
433 
434  // AHRS Message Format
435  // Set message format
436  ROS_INFO("Setting the AHRS message format");
437  data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED;
438  data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED;
439  data_stream_format_descriptors[2] = MIP_AHRS_DATA_QUATERNION;
440  data_stream_format_decimation[0] = imu_decimation; // 0x32;
441  data_stream_format_decimation[1] = imu_decimation; // 0x32;
442  data_stream_format_decimation[2] = imu_decimation; // 0x32;
443  data_stream_format_num_entries = 3;
444 
445  start = clock();
446  while (mip_3dm_cmd_ahrs_message_format(&device_interface_,
447  MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
448  data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK)
449  {
450  if (clock() - start > 5000)
451  {
452  ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out.");
453  break;
454  }
455  }
456 
457  ros::Duration(dT).sleep();
458  // Poll to verify
459  ROS_INFO("Poll AHRS data to verify");
460  start = clock();
461  while (mip_3dm_cmd_poll_ahrs(&device_interface_,
462  MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries,
463  data_stream_format_descriptors) != MIP_INTERFACE_OK)
464  {
465  if (clock() - start > 5000)
466  {
467  ROS_INFO("mip_3dm_cmd_poll_ahrs function timed out.");
468  break;
469  }
470  }
471 
472  ros::Duration(dT).sleep();
473  // Save
474  if (save_settings)
475  {
476  ROS_INFO("Saving AHRS data settings");
477  start = clock();
478  while (mip_3dm_cmd_ahrs_message_format(&device_interface_,
479  MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK)
480  {
481  if (clock() - start > 5000)
482  {
483  ROS_INFO("mip_3dm_cmd_ahrs_message_format function timed out.");
484  break;
485  }
486  }
487  ros::Duration(dT).sleep();
488  }
489 
490  // Declination Source
491  // Set declination
492  ROS_INFO("Setting declination source to %#04X", declination_source_u8);
493  start = clock();
494  while (mip_filter_declination_source(&device_interface_,
495  MIP_FUNCTION_SELECTOR_WRITE, &declination_source_u8) != MIP_INTERFACE_OK)
496  {
497  if (clock() - start > 5000)
498  {
499  ROS_INFO("mip_filter_declination_source function timed out.");
500  break;
501  }
502  }
503 
504  ros::Duration(dT).sleep();
505  // Read back the declination source
506  ROS_INFO("Reading back declination source");
507  start = clock();
508  while (mip_filter_declination_source(&device_interface_,
509  MIP_FUNCTION_SELECTOR_READ, &readback_declination_source) != MIP_INTERFACE_OK)
510  {
511  if (clock() - start > 5000)
512  {
513  ROS_INFO("mip_filter_declination_source function timed out.");
514  break;
515  }
516  }
517 
518  if (declination_source_u8 == readback_declination_source)
519  {
520  ROS_INFO("Success: Declination source set to %#04X", declination_source_u8);
521  }
522  else
523  {
524  ROS_WARN("Failed to set the declination source to %#04X!", declination_source_u8);
525  }
526 
527  ros::Duration(dT).sleep();
528  if (save_settings)
529  {
530  ROS_INFO("Saving declination source settings to EEPROM");
531  start = clock();
532  while (mip_filter_declination_source(&device_interface_,
533  MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK)
534  {
535  if (clock() - start > 5000)
536  {
537  ROS_INFO("mip_filter_declination_source function timed out.");
538  break;
539  }
540  }
541 
542  ros::Duration(dT).sleep();
543  }
544  } // end of AHRS setup
545 
546 
547  // GPS Setup
548  if (publish_gps_)
549  {
550  start = clock();
551  while (mip_3dm_cmd_get_gps_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK)
552  {
553  if (clock() - start > 5000)
554  {
555  ROS_INFO("mip_3dm_cmd_get_gps_base_rate function timed out.");
556  break;
557  }
558  }
559 
560  ROS_INFO("GPS Base Rate => %d Hz", base_rate);
561  u8 gps_decimation = (u8)(static_cast<float>(base_rate)/ static_cast<float>(gps_rate_));
562  ros::Duration(dT).sleep();
563 
565  // Set
566  ROS_INFO("Setting GPS stream format");
567  data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS;
568  data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY;
569  data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME;
570  data_stream_format_decimation[0] = gps_decimation; // 0x01; // 0x04;
571  data_stream_format_decimation[1] = gps_decimation; // 0x01; // 0x04;
572  data_stream_format_decimation[2] = gps_decimation; // 0x01; // 0x04;
573  data_stream_format_num_entries = 3;
574  start = clock();
575 
576  while (mip_3dm_cmd_gps_message_format(&device_interface_,
577  MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
578  data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK)
579  {
580  if (clock() - start > 5000)
581  {
582  ROS_INFO("mip_3dm_cmd_gps_message_format function timed out.");
583  break;
584  }
585  }
586 
587  ros::Duration(dT).sleep();
588  // Save
589  if (save_settings)
590  {
591  ROS_INFO("Saving GPS data settings");
592  start = clock();
593  while (mip_3dm_cmd_gps_message_format(&device_interface_,
594  MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK)
595  {
596  if (clock() - start > 5000)
597  {
598  ROS_INFO("mip_3dm_cmd_gps_message_format function timed out.");
599  break;
600  }
601  }
602  ros::Duration(dT).sleep();
603  }
604  } // end of GPS setup
605 
606  // Filter setup
607  if (publish_odom_ || publish_filtered_imu_)
608  {
609  start = clock();
610  while (mip_3dm_cmd_get_filter_base_rate(&device_interface_, &base_rate) != MIP_INTERFACE_OK)
611  {
612  if (clock() - start > 5000)
613  {
614  ROS_INFO("mip_3dm_cmd_get_filter_base_rate function timed out.");
615  break;
616  }
617  }
618 
619  ROS_INFO("FILTER Base Rate => %d Hz", base_rate);
620  // If we have made it this far in this statement, we know we want to publish filtered data
621  // from the IMU. We need to make sure to set the data rate to the correct speed dependent
622  // upon which filtered field we are after. Thus make sure we get the fastest data rate.
623  int rate = nav_rate_;
624  if(publish_filtered_imu_)
625  {
626  // Set filter rate based on max of filter topic rates
627  rate = (imu_rate_ > nav_rate_) ? imu_rate_ : nav_rate_;
628  }
629 
630  u8 nav_decimation = (u8)(static_cast<float>(base_rate)/ static_cast<float>(rate));
631  ros::Duration(dT).sleep();
632 
634  // Set
635  ROS_INFO("Setting Filter stream format");
636 
637  // Order doesn't matter since we parse them with a case statement below.
638  // First start by loading the common values.
639  data_stream_format_descriptors[0] = MIP_FILTER_DATA_ATT_QUATERNION;
640  data_stream_format_descriptors[1] = MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER;
641  data_stream_format_descriptors[2] = MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE;
642  data_stream_format_descriptors[3] = MIP_FILTER_DATA_FILTER_STATUS;
643  data_stream_format_decimation[0] = nav_decimation; // 0x32;
644  data_stream_format_decimation[1] = nav_decimation; // 0x32;
645  data_stream_format_decimation[2] = nav_decimation; // 0x32;
646  data_stream_format_decimation[3] = nav_decimation; // 0x32;
647 
648  // If we want the odometry add that data
649  if (publish_odom_ && publish_filtered_imu_)
650  {
651  // Size is up to 10 elements
652  data_stream_format_descriptors[4] = MIP_FILTER_DATA_LLH_POS;
653  data_stream_format_descriptors[5] = MIP_FILTER_DATA_NED_VEL;
654  // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES;
655  data_stream_format_descriptors[6] = MIP_FILTER_DATA_POS_UNCERTAINTY;
656  data_stream_format_descriptors[7] = MIP_FILTER_DATA_VEL_UNCERTAINTY;
657 
658  // The filter has one message that removes gravity and one that does not
659  if (remove_imu_gravity_)
660  {
661  data_stream_format_descriptors[8] = MIP_FILTER_DATA_LINEAR_ACCELERATION;
662  }
663  else
664  {
665  data_stream_format_descriptors[8] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION;
666  }
667 
668  data_stream_format_decimation[4] = nav_decimation; // 0x32;
669  data_stream_format_decimation[5] = nav_decimation; // 0x32;
670  data_stream_format_decimation[6] = nav_decimation; // 0x32;
671  data_stream_format_decimation[7] = nav_decimation; // 0x32;
672  data_stream_format_decimation[8] = nav_decimation; // 0x32;
673  data_stream_format_num_entries = 9;
674  }
675  else if (publish_odom_ && !publish_filtered_imu_)
676  {
677  data_stream_format_descriptors[4] = MIP_FILTER_DATA_LLH_POS;
678  data_stream_format_descriptors[5] = MIP_FILTER_DATA_NED_VEL;
679  // data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES;
680  data_stream_format_descriptors[6] = MIP_FILTER_DATA_POS_UNCERTAINTY;
681  data_stream_format_descriptors[7] = MIP_FILTER_DATA_VEL_UNCERTAINTY;
682 
683  data_stream_format_decimation[4] = nav_decimation; // 0x32;
684  data_stream_format_decimation[5] = nav_decimation; // 0x32;
685  data_stream_format_decimation[6] = nav_decimation; // 0x32;
686  data_stream_format_decimation[7] = nav_decimation; // 0x32;
687  data_stream_format_num_entries = 8;
688  }
689  else
690  {
691  // The filter has one message that removes gravity and one that does not
692  if (remove_imu_gravity_)
693  {
694  data_stream_format_descriptors[4] = MIP_FILTER_DATA_LINEAR_ACCELERATION;
695  }
696  else
697  {
698  data_stream_format_descriptors[4] = MIP_FILTER_DATA_COMPENSATED_ACCELERATION;
699  }
700  data_stream_format_decimation[4] = nav_decimation; // 0x32;
701  data_stream_format_num_entries = 5;
702  }
703 
704  start = clock();
705  while (mip_3dm_cmd_filter_message_format(&device_interface_,
706  MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
707  data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK)
708  {
709  if (clock() - start > 5000)
710  {
711  ROS_INFO("mip_3dm_cmd_filter_message_format function timed out.");
712  break;
713  }
714  }
715 
716  ros::Duration(dT).sleep();
717  // Poll to verify
718  ROS_INFO("Poll filter data to test stream");
719  start = clock();
720  while (mip_3dm_cmd_poll_filter(&device_interface_,
721  MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries,
722  data_stream_format_descriptors) != MIP_INTERFACE_OK)
723  {
724  if (clock() - start > 5000)
725  {
726  ROS_INFO("mip_3dm_cmd_poll_filter function timed out.");
727  break;
728  }
729  }
730 
731  ros::Duration(dT).sleep();
732  // Save
733  if (save_settings)
734  {
735  ROS_INFO("Saving Filter data settings");
736  start = clock();
737  while (mip_3dm_cmd_filter_message_format(&device_interface_,
738  MIP_FUNCTION_SELECTOR_STORE_EEPROM, 0, NULL, NULL) != MIP_INTERFACE_OK)
739  {
740  if (clock() - start > 5000)
741  {
742  ROS_INFO("mip_3dm_cmd_filter_message_format function timed out.");
743  break;
744  }
745  }
746  ros::Duration(dT).sleep();
747  }
748 
749 
750  // GX5_25 doesn't appear to suport this feature thus GX5_15 probably won't either
751  if (GX5_35 == true || GX5_45 == true)
752  {
753  // Dynamics Mode
754  // Set dynamics mode
755  ROS_INFO("Setting dynamics mode to %#04X", dynamics_mode);
756  start = clock();
757  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
758  MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK)
759  {
760  if (clock() - start > 5000)
761  {
762  ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out.");
763  break;
764  }
765  }
766 
767  ros::Duration(dT).sleep();
768  // Readback dynamics mode
769  if (readback_settings)
770  {
771  // Read the settings back
772  ROS_INFO("Reading back dynamics mode setting");
773  start = clock();
774  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
775  MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK)
776  {
777  if (clock() - start > 5000)
778  {
779  ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out.");
780  break;
781  }
782  }
783 
784  ros::Duration(dT).sleep();
785  if (dynamics_mode == readback_dynamics_mode)
786  ROS_INFO("Success: Dynamics mode setting is: %#04X", readback_dynamics_mode);
787  else
788  ROS_ERROR("Failure: Dynamics mode set to be %#04X, but reads as %#04X",
789  dynamics_mode, readback_dynamics_mode);
790  }
791 
792  if (save_settings)
793  {
794  ROS_INFO("Saving dynamics mode settings to EEPROM");
795  start = clock();
796  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
797  MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK)
798  {
799  if (clock() - start > 5000)
800  {
801  ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out.");
802  break;
803  }
804  }
805  ros::Duration(dT).sleep();
806  }
807  }
808 
809  // Set heading Source
810  ROS_INFO("Set heading source to internal mag.");
811  heading_source = 0x1;
812  ROS_INFO("Setting heading source to %#04X", heading_source);
813  start = clock();
814  while (mip_filter_heading_source(&device_interface_,
815  MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK)
816  {
817  if (clock() - start > 5000)
818  {
819  ROS_INFO("mip_filter_heading_source function timed out.");
820  break;
821  }
822  }
823  // Read back heading source
824  ros::Duration(dT).sleep();
825  ROS_INFO("Read back heading source...");
826  start = clock();
827  while (mip_filter_heading_source(&device_interface_,
828  MIP_FUNCTION_SELECTOR_READ, &readback_headingsource)!= MIP_INTERFACE_OK)
829  {
830  if (clock() - start > 5000)
831  {
832  ROS_INFO("mip_filter_heading_source function timed out.");
833  break;
834  }
835  }
836 
837  ROS_INFO("Heading source = %#04X", readback_headingsource);
838  ros::Duration(dT).sleep();
839 
840  if (save_settings)
841  {
842  ROS_INFO("Saving heading source to EEPROM");
843  start = clock();
844  while (mip_filter_heading_source(&device_interface_,
845  MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL)!= MIP_INTERFACE_OK)
846  {
847  if (clock() - start > 5000)
848  {
849  ROS_INFO("mip_filter_heading_source function timed out.");
850  break;
851  }
852  }
853  ros::Duration(dT).sleep();
854  }
855  } // end of Filter setup
856 
857  // Set auto-initialization based on ROS parameter
858  ROS_INFO("Setting auto-initinitalization to: %#04X", auto_init);
859  auto_init_u8 = auto_init; // convert bool to u8
860  start = clock();
861  while (mip_filter_auto_initialization(&device_interface_,
862  MIP_FUNCTION_SELECTOR_WRITE, &auto_init_u8) != MIP_INTERFACE_OK)
863  {
864  if (clock() - start > 5000)
865  {
866  ROS_INFO("mip_filter_auto_initialization function timed out.");
867  break;
868  }
869  }
870  ros::Duration(dT).sleep();
871 
872  if (readback_settings)
873  {
874  // Read the settings back
875  ROS_INFO("Reading back auto-initialization value");
876  start = clock();
877  while (mip_filter_auto_initialization(&device_interface_,
878  MIP_FUNCTION_SELECTOR_READ, &readback_auto_init)!= MIP_INTERFACE_OK)
879  {
880  if (clock() - start > 5000)
881  {
882  ROS_INFO("mip_filter_auto_initialization function timed out.");
883  break;
884  }
885  }
886 
887  ros::Duration(dT).sleep();
888  if (auto_init == readback_auto_init)
889  ROS_INFO("Success: Auto init. setting is: %#04X",
890  readback_auto_init);
891  else
892  ROS_ERROR("Failure: Auto init. setting set to be %#04X, but reads as %#04X",
893  auto_init, readback_auto_init);
894  }
895 
896  if (save_settings)
897  {
898  ROS_INFO("Saving auto init. settings to EEPROM");
899  start = clock();
900  while (mip_filter_auto_initialization(&device_interface_,
901  MIP_FUNCTION_SELECTOR_STORE_EEPROM, NULL) != MIP_INTERFACE_OK)
902  {
903  if (clock() - start > 5000)
904  {
905  ROS_INFO("mip_filter_auto_initialization function timed out.");
906  break;
907  }
908  }
909  ros::Duration(dT).sleep();
910  }
911 
912  // Enable Data streams
913  dT = 0.25;
914  if (publish_imu_ || publish_filtered_imu_)
915  {
916  ROS_INFO("Enabling AHRS stream");
917  enable = 0x01;
918  start = clock();
919  while (mip_3dm_cmd_continuous_data_stream(&device_interface_,
920  MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK)
921  {
922  if (clock() - start > 5000)
923  {
924  ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out.");
925  break;
926  }
927  }
928  ros::Duration(dT).sleep();
929  }
930 
931  if (publish_odom_)
932  {
933  ROS_INFO("Enabling Filter stream");
934  enable = 0x01;
935  start = clock();
936  while (mip_3dm_cmd_continuous_data_stream(&device_interface_,
937  MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK)
938  {
939  if (clock() - start > 5000)
940  {
941  ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out.");
942  break;
943  }
944  }
945  ros::Duration(dT).sleep();
946  }
947 
948  if (publish_gps_)
949  {
950  ROS_INFO("Enabling GPS stream");
951  enable = 0x01;
952  start = clock();
953  while (mip_3dm_cmd_continuous_data_stream(&device_interface_,
954  MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK)
955  {
956  if (clock() - start > 5000)
957  {
958  ROS_INFO("mip_3dm_cmd_continuous_data_stream function timed out.");
959  break;
960  }
961  }
962  ros::Duration(dT).sleep();
963  }
964 
965  ROS_INFO("End of device setup - starting streaming");
966  }
967  else
968  {
969  ROS_INFO("Skipping device setup and listing for existing streams");
970  } // end of device_setup
971 
972  // Reset filter - should be for either the KF or CF
973  ROS_INFO("Reset filter");
974  start = clock();
975  while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK)
976  {
977  if (clock() - start > 5000)
978  {
979  ROS_INFO("mip_filter_reset_filter function timed out.");
980  break;
981  }
982  }
983  ros::Duration(dT).sleep();
984 
985  // Loop
986  // Determine loop rate as 2*(max update rate), but abs. max of 1kHz
987  int max_rate = 1;
988  if (publish_imu_)
989  {
990  max_rate = std::max(max_rate, imu_rate_);
991  }
992  if (publish_filtered_imu_)
993  {
994  max_rate = std::max(std::max(max_rate, nav_rate_),imu_rate_);
995  }
996  if (publish_gps_)
997  {
998  max_rate = std::max(max_rate, gps_rate_);
999  }
1000  if (publish_odom_)
1001  {
1002  max_rate = std::max(max_rate, nav_rate_);
1003  }
1004  int spin_rate = std::min(3*max_rate, 1000);
1005  ROS_INFO("Setting spin rate to <%d>", spin_rate);
1006  ros::Rate r(spin_rate); // Rate in Hz
1007 
1008  microstrain_mips::RosDiagnosticUpdater ros_diagnostic_updater(this);
1009 
1010  while (ros::ok())
1011  {
1012  // Update the parser (this function reads the port and parses the bytes
1013  mip_interface_update(&device_interface_);
1014 
1015  if (GX5_25)
1016  {
1017  device_status_callback();
1018  }
1019 
1020  ros::spinOnce(); // take care of service requests.
1021  r.sleep();
1022  } // end loop
1023 
1024  // close serial port
1025  mip_sdk_port_close(device_interface_.port_handle);
1026  } // End of ::run()
1027 
1028  bool Microstrain::reset_callback(std_srvs::Empty::Request &req,
1029  std_srvs::Empty::Response &resp)
1030  {
1031  ROS_INFO("Reseting the filter");
1032  start = clock();
1033  while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK)
1034  {
1035  if (clock() - start > 5000)
1036  {
1037  ROS_INFO("mip_filter_reset_filter function timed out.");
1038  break;
1039  }
1040  }
1041  return true;
1042  }
1043 
1044  // Services to get/set values on devices
1045  // Set accel bias values
1046  bool Microstrain::set_accel_bias(microstrain_mips::SetAccelBias::Request &req,
1047  microstrain_mips::SetAccelBias::Response &res)
1048  {
1049  ROS_INFO("Setting accel bias values");
1050  memset(field_data, 0, 3*sizeof(float));
1051  start = clock();
1052  while (mip_3dm_cmd_accel_bias(&device_interface_,
1053  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1054  {
1055  if (clock() - start > 5000)
1056  {
1057  ROS_INFO("mip_3dm_cmd_accel_bias function timed out.");
1058  break;
1059  }
1060  }
1061 
1062  ROS_INFO("Accel bias vector values are: %f %f %f",
1063  field_data[0], field_data[1], field_data[2]);
1064  ROS_INFO("Client request values are: %.2f %.2f %.2f",
1065  req.bias.x, req.bias.y, req.bias.z);
1066 
1067  field_data[0] = req.bias.x;
1068  field_data[1] = req.bias.y;
1069  field_data[2] = req.bias.z;
1070 
1071  start = clock();
1072  while (mip_3dm_cmd_accel_bias(&device_interface_,
1073  MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK)
1074  {
1075  if (clock() - start > 5000)
1076  {
1077  ROS_INFO("mip_3dm_cmd_accel_bias function timed out.");
1078  break;
1079  }
1080  }
1081 
1082  memset(field_data, 0, 3*sizeof(float));
1083  start = clock();
1084  while (mip_3dm_cmd_accel_bias(&device_interface_,
1085  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1086  {
1087  if (clock() - start > 5000)
1088  {
1089  ROS_INFO("mip_3dm_cmd_accel_bias function timed out.");
1090  break;
1091  }
1092  }
1093  ROS_INFO("New accel bias vector values are: %.2f %.2f %.2f",
1094  field_data[0], field_data[1], field_data[2]);
1095 
1096  res.success = true;
1097  return true;
1098  }
1099 
1100  // Get accel bias values
1101  bool Microstrain::get_accel_bias(std_srvs::Trigger::Request &req,
1102  std_srvs::Trigger::Response &res)
1103  {
1104  ROS_INFO("Getting accel bias values");
1105  memset(field_data, 0, 3*sizeof(float));
1106 
1107  start = clock();
1108  while (mip_3dm_cmd_accel_bias(&device_interface_,
1109  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1110  {
1111  if (clock() - start > 5000)
1112  {
1113  ROS_INFO("mip_3dm_cmd_accel_bias function timed out.");
1114  break;
1115  }
1116  }
1117  ROS_INFO("Accel bias vector values are: %f %f %f",
1118  field_data[0], field_data[1], field_data[2]);
1119 
1120  res.success = true;
1121  return true;
1122  }
1123 
1124  // Set gyro bias values
1125  bool Microstrain::set_gyro_bias(microstrain_mips::SetGyroBias::Request &req,
1126  microstrain_mips::SetGyroBias::Response &res)
1127  {
1128  ROS_INFO("Setting gyro bias values");
1129  memset(field_data, 0, 3*sizeof(float));
1130 
1131  start = clock();
1132  while (mip_3dm_cmd_gyro_bias(&device_interface_,
1133  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1134  {
1135  if (clock() - start > 5000)
1136  {
1137  ROS_INFO("mip_3dm_cmd_gyro_bias function timed out.");
1138  break;
1139  }
1140  }
1141 
1142  ROS_INFO("Gyro bias vector values are: %f %f %f",
1143  field_data[0], field_data[1], field_data[2]);
1144  ROS_INFO("Client request values are: %.2f %.2f %.2f",
1145  req.bias.x, req.bias.y, req.bias.z);
1146 
1147  field_data[0] = req.bias.x;
1148  field_data[1] = req.bias.y;
1149  field_data[2] = req.bias.z;
1150 
1151  start = clock();
1152  while (mip_3dm_cmd_gyro_bias(&device_interface_,
1153  MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK)
1154  {
1155  if (clock() - start > 5000)
1156  {
1157  ROS_INFO("mip_3dm_cmd_gyro_bias function timed out.");
1158  break;
1159  }
1160  }
1161  memset(field_data, 0, 3*sizeof(float));
1162  start = clock();
1163  while (mip_3dm_cmd_gyro_bias(&device_interface_,
1164  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1165  {
1166  if (clock() - start > 5000)
1167  {
1168  ROS_INFO("mip_3dm_cmd_gyro_bias function timed out.");
1169  break;
1170  }
1171  }
1172  ROS_INFO("New gyro bias vector values are: %.2f %.2f %.2f",
1173  field_data[0], field_data[1], field_data[2]);
1174 
1175  res.success = true;
1176  return true;
1177  }
1178 
1179  // Get gyro bias values
1180  bool Microstrain::get_gyro_bias(std_srvs::Trigger::Request &req,
1181  std_srvs::Trigger::Response &res)
1182  {
1183  ROS_INFO("Getting gyro bias values");
1184  memset(field_data, 0, 3*sizeof(float));
1185 
1186  start = clock();
1187  while (mip_3dm_cmd_gyro_bias(&device_interface_,
1188  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1189  {
1190  if (clock() - start > 5000)
1191  {
1192  ROS_INFO("mip_3dm_cmd_gyro_bias function timed out.");
1193  break;
1194  }
1195  }
1196  ROS_INFO("Gyro bias vector values are: %f %f %f", field_data[0], field_data[1], field_data[2]);
1197 
1198  res.success = true;
1199  return true;
1200  }
1201 
1202  // Set hard iron values
1203  bool Microstrain::set_hard_iron_values(microstrain_mips::SetHardIronValues::Request &req,
1204  microstrain_mips::SetHardIronValues::Response &res)
1205  {
1206  if (GX5_15 == true)
1207  {
1208  ROS_INFO("Device does not support this feature");
1209  res.success = false;
1210  return true;
1211  }
1212 
1213  ROS_INFO("Setting hard iron values");
1214  float field_data[3] = {0};
1215 
1216  start = clock();
1217  while (mip_3dm_cmd_hard_iron(&device_interface_,
1218  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1219  {
1220  if (clock() - start > 5000)
1221  {
1222  ROS_INFO("mip_3dm_cmd_hard_iron function timed out.");
1223  break;
1224  }
1225  }
1226 
1227  ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]);
1228  ROS_INFO("Client request values are: %.2f %.2f %.2f", req.bias.x, req.bias.y, req.bias.z);
1229 
1230  field_data[0] = req.bias.x;
1231  field_data[1] = req.bias.y;
1232  field_data[2] = req.bias.z;
1233 
1234  start = clock();
1235  while (mip_3dm_cmd_hard_iron(&device_interface_,
1236  MIP_FUNCTION_SELECTOR_WRITE, field_data) != MIP_INTERFACE_OK)
1237  {
1238  if (clock() - start > 5000)
1239  {
1240  ROS_INFO("mip_3dm_cmd_hard_iron function timed out.");
1241  break;
1242  }
1243  }
1244 
1245  memset(field_data, 0, 3*sizeof(float));
1246  start = clock();
1247  while (mip_3dm_cmd_hard_iron(&device_interface_,
1248  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1249  {
1250  if (clock() - start > 5000)
1251  {
1252  ROS_INFO("mip_3dm_cmd_hard_iron function timed out.");
1253  break;
1254  }
1255  }
1256  ROS_INFO("New hard iron values are: %.2f %.2f %.2f", field_data[0], field_data[1], field_data[2]);
1257 
1258  res.success = true;
1259  return true;
1260  }
1261 
1262  // Get hard iron values
1263  bool Microstrain::get_hard_iron_values(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
1264  {
1265  if (GX5_15 == true)
1266  {
1267  ROS_INFO("Device does not support this feature");
1268  res.success = false;
1269  return true;
1270  }
1271 
1272  ROS_INFO("Getting hard iron values");
1273  memset(field_data, 0, 3*sizeof(float));
1274 
1275  start = clock();
1276  while (mip_3dm_cmd_hard_iron(&device_interface_,
1277  MIP_FUNCTION_SELECTOR_READ, field_data) != MIP_INTERFACE_OK)
1278  {
1279  if (clock() - start > 5000)
1280  {
1281  ROS_INFO("mip_3dm_cmd_hard_iron function timed out.");
1282  break;
1283  }
1284  }
1285  ROS_INFO("Hard iron values are: %f %f %f", field_data[0], field_data[1], field_data[2]);
1286 
1287  res.success = true;
1288  return true;
1289  }
1290 
1291 
1292  // Get device report
1293  bool Microstrain::device_report(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
1294  {
1295  start = clock();
1296  while (mip_base_cmd_get_device_info(&device_interface_, &device_info) != MIP_INTERFACE_OK)
1297  {
1298  if (clock() - start > 5000)
1299  {
1300  ROS_INFO("mip_base_cmd_get_device_info function timed out.");
1301  break;
1302  }
1303  }
1304 
1305  ROS_INFO("\n\nDevice Info:\n");
1306 
1307  memset(temp_string, 0, 20*sizeof(int));
1308 
1309  memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
1310  ROS_INFO("Model Name => %s\n", temp_string);
1311 
1312  memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
1313  ROS_INFO("Model Number => %s\n", temp_string);
1314 
1315  memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
1316  ROS_INFO("Serial Number => %s\n", temp_string);
1317 
1318  memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2);
1319  ROS_INFO("Lot Number => %s\n", temp_string);
1320 
1321  memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2);
1322  ROS_INFO("Options => %s\n", temp_string);
1323 
1324  ROS_INFO("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000,
1325  (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100);
1326 
1327  res.success = true;
1328  return true;
1329  }
1330 
1331  // Capture gyro bias values
1332  bool Microstrain::gyro_bias_capture(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
1333  {
1334  memset(field_data, 0, 3*sizeof(float));
1335  ROS_INFO("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n");
1336  duration = 5000; // milliseconds
1337  start = clock();
1338  while (mip_3dm_cmd_capture_gyro_bias(&device_interface_, duration, field_data) != MIP_INTERFACE_OK)
1339  {
1340  if (clock() - start > 5000)
1341  {
1342  ROS_INFO("mip_3dm_cmd_capture_gyro_bias function timed out.");
1343  break;
1344  }
1345  }
1346  ROS_INFO("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n",
1347  field_data[0], field_data[1], field_data[2]);
1348 
1349  res.success = true;
1350  return true;
1351  }
1352 
1353  // Set soft iron matrix values
1354  bool Microstrain::set_soft_iron_matrix(microstrain_mips::SetSoftIronMatrix::Request &req,
1355  microstrain_mips::SetSoftIronMatrix::Response &res)
1356  {
1357  if (GX5_15 == true)
1358  {
1359  ROS_INFO("Device does not support this feature");
1360  res.success = false;
1361  return true;
1362  }
1363 
1364  memset(soft_iron, 0, 9*sizeof(float));
1365  memset(soft_iron_readback, 0, 9*sizeof(float));
1366 
1367  ROS_INFO("Setting the soft iron matrix values\n");
1368 
1369  soft_iron[0] = req.soft_iron_1.x;
1370  soft_iron[1] = req.soft_iron_1.y;
1371  soft_iron[2] = req.soft_iron_1.z;
1372  soft_iron[3] = req.soft_iron_2.x;
1373  soft_iron[4] = req.soft_iron_2.y;
1374  soft_iron[5] = req.soft_iron_2.z;
1375  soft_iron[6] = req.soft_iron_3.x;
1376  soft_iron[7] = req.soft_iron_3.y;
1377  soft_iron[8] = req.soft_iron_3.z;
1378 
1379  start = clock();
1380  while (mip_3dm_cmd_soft_iron(&device_interface_,
1381  MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK)
1382  {
1383  if (clock() - start > 5000)
1384  {
1385  ROS_INFO("mip_3dm_cmd_soft_iron function timed out.");
1386  break;
1387  }
1388  }
1389 
1390  // Read back the soft iron matrix values
1391  start = clock();
1392  while (mip_3dm_cmd_soft_iron(&device_interface_,
1393  MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK)
1394  {
1395  if (clock() - start > 5000)
1396  {
1397  ROS_INFO("mip_3dm_cmd_soft_iron function timed out.");
1398  break;
1399  }
1400  }
1401 
1402  if ((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) &&
1403  (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) &&
1404  (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) &&
1405  (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) &&
1406  (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) &&
1407  (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) &&
1408  (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) &&
1409  (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) &&
1410  (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001))
1411  {
1412  ROS_INFO("Soft iron matrix values successfully set.\n");
1413  ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n",
1414  soft_iron[0], soft_iron[1], soft_iron[2],
1415  soft_iron[3], soft_iron[4], soft_iron[5],
1416  soft_iron[6], soft_iron[7], soft_iron[8]);
1417  ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n",
1418  soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2],
1419  soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5],
1420  soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
1421  }
1422  else
1423  {
1424  ROS_INFO("ERROR: Failed to set hard iron values!!!\n");
1425  ROS_INFO("Sent values: [%f %f %f][%f %f %f][%f %f %f]\n",
1426  soft_iron[0], soft_iron[1], soft_iron[2],
1427  soft_iron[3], soft_iron[4], soft_iron[5],
1428  soft_iron[6], soft_iron[7], soft_iron[8]);
1429  ROS_INFO("Returned values: [%f %f %f][%f %f %f][%f %f %f]\n",
1430  soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2],
1431  soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5],
1432  soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
1433  }
1434  res.success = true;
1435  return true;
1436  }
1437 
1438  // Get soft iron matrix values
1439  bool Microstrain::get_soft_iron_matrix(std_srvs::Trigger::Request &req,
1440  std_srvs::Trigger::Response &res)
1441  {
1442  if (GX5_15 == true)
1443  {
1444  ROS_INFO("Device does not support this feature");
1445  res.success = false;
1446  return true;
1447  }
1448 
1449  memset(soft_iron, 0, 9*sizeof(float));
1450  memset(soft_iron_readback, 0, 9*sizeof(float));
1451 
1452  ROS_INFO("Getting the soft iron matrix values\n");
1453 
1454  start = clock();
1455  while (mip_3dm_cmd_soft_iron(&device_interface_,
1456  MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK)
1457  {
1458  if (clock() - start > 5000)
1459  {
1460  ROS_INFO("mip_3dm_cmd_soft_iron function timed out.");
1461  break;
1462  }
1463  }
1464 
1465  ROS_INFO("Soft iron matrix values: [%f %f %f][%f %f %f][%f %f %f]\n",
1466  soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2],
1467  soft_iron_readback[3], soft_iron_readback[4], soft_iron_readback[5],
1468  soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
1469  res.success = true;
1470  return true;
1471  }
1472 
1473  // Set complementary filter values
1474  bool Microstrain::set_complementary_filter(microstrain_mips::SetComplementaryFilter::Request &req,
1475  microstrain_mips::SetComplementaryFilter::Response &res)
1476  {
1477  ROS_INFO("Setting the complementary filter values\n");
1478 
1479  comp_filter_command.north_compensation_enable = req.north_comp_enable;
1480  comp_filter_command.up_compensation_enable = req.up_comp_enable;
1481  comp_filter_command.north_compensation_time_constant = req.north_comp_time_const;
1482  comp_filter_command.up_compensation_time_constant = req.up_comp_time_const;
1483 
1484  start = clock();
1485  while (mip_3dm_cmd_complementary_filter_settings(&device_interface_,
1486  MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK)
1487  {
1488  if (clock() - start > 5000)
1489  {
1490  ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out.");
1491  break;
1492  }
1493  }
1494 
1495  // Read back the complementary filter values
1496  start = clock();
1497  while (mip_3dm_cmd_complementary_filter_settings(&device_interface_,
1498  MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK)
1499  {
1500  if (clock() - start > 5000)
1501  {
1502  ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out.");
1503  break;
1504  }
1505  }
1506 
1507  if ((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) &&
1508  (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) &&
1509  (abs(comp_filter_command.north_compensation_time_constant -
1510  comp_filter_readback.north_compensation_time_constant) < 0.001) &&
1511  (abs(comp_filter_command.up_compensation_time_constant -
1512  comp_filter_readback.up_compensation_time_constant) < 0.001))
1513  {
1514  ROS_INFO("Complementary filter values successfully set.\n");
1515  ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n",
1516  comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable,
1517  comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant);
1518  ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n",
1519  comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable,
1520  comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant);
1521  }
1522  else
1523  {
1524  ROS_INFO("ERROR: Failed to set complementary filter values!!!\n");
1525  ROS_INFO("Sent values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n",
1526  comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable,
1527  comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant);
1528  ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n",
1529  comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable,
1530  comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant);
1531  }
1532  res.success = true;
1533  return true;
1534  }
1535 
1536  // Get complementary filter values
1537  bool Microstrain::get_complementary_filter(std_srvs::Trigger::Request &req,
1538  std_srvs::Trigger::Response &res)
1539  {
1540  // Read back the complementary filter values
1541  start = clock();
1542  while (mip_3dm_cmd_complementary_filter_settings(&device_interface_,
1543  MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK)
1544  {
1545  if (clock() - start > 5000)
1546  {
1547  ROS_INFO("mip_3dm_cmd_complementary_filter_settings function timed out.");
1548  break;
1549  }
1550  }
1551  ROS_INFO("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n",
1552  comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable,
1553  comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant);
1554  res.success = true;
1555  return true;
1556  }
1557 
1558  // Initialize filter with Euler angles
1559  bool Microstrain::set_filter_euler(microstrain_mips::SetFilterEuler::Request &req,
1560  microstrain_mips::SetFilterEuler::Response &res)
1561  {
1562  memset(angles, 0, 3*sizeof(float));
1563  ROS_INFO("Resetting the Filter\n");
1564 
1565  start = clock();
1566  while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK)
1567  {
1568  if (clock() - start > 5000)
1569  {
1570  ROS_INFO("mip_filter_reset_filter function timed out.");
1571  break;
1572  }
1573  }
1574 
1575  ROS_INFO("Initializing the Filter with Euler angles\n");
1576 
1577  angles[0] = req.angle.x;
1578  angles[1] = req.angle.y;
1579  angles[2] = req.angle.z;
1580 
1581  start = clock();
1582  while (mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK)
1583  {
1584  if (clock() - start > 5000)
1585  {
1586  ROS_INFO("mip_filter_set_init_attitude function timed out.");
1587  break;
1588  }
1589  }
1590 
1591  res.success = true;
1592  return true;
1593  }
1594 
1595  // Set filter with heading angle
1596  bool Microstrain::set_filter_heading(microstrain_mips::SetFilterHeading::Request &req,
1597  microstrain_mips::SetFilterHeading::Response &res)
1598  {
1599  ROS_INFO("Resetting the Filter\n");
1600 
1601  start = clock();
1602  while (mip_filter_reset_filter(&device_interface_) != MIP_INTERFACE_OK)
1603  {
1604  if (clock() - start > 5000)
1605  {
1606  ROS_INFO("mip_filter_reset_filter function timed out.");
1607  break;
1608  }
1609  }
1610 
1611  ROS_INFO("Initializing the Filter with a heading angle\n");
1612 
1613  heading_angle = req.angle;
1614  start = clock();
1615  while (mip_filter_set_init_heading(&device_interface_, heading_angle) != MIP_INTERFACE_OK)
1616  {
1617  if (clock() - start > 5000)
1618  {
1619  ROS_INFO("mip_filter_set_init_heading function timed out.");
1620  break;
1621  }
1622  }
1623 
1624  res.success = true;
1625  return true;
1626  }
1627 
1628 
1629  // Set sensor to vehicle frame transformation
1630  bool Microstrain::set_sensor_vehicle_frame_trans(microstrain_mips::SetSensorVehicleFrameTrans::Request &req,
1631  microstrain_mips::SetSensorVehicleFrameTrans::Response &res)
1632  {
1633  if (GX5_15 == true)
1634  {
1635  ROS_INFO("Device does not support this feature");
1636  res.success = false;
1637  return true;
1638  }
1639 
1640  memset(angles, 0, 3*sizeof(float));
1641  memset(readback_angles, 0, 3*sizeof(float));
1642 
1643  ROS_INFO("Setting the sensor to vehicle frame transformation\n");
1644 
1645  angles[0] = req.angle.x;
1646  angles[1] = req.angle.y;
1647  angles[2] = req.angle.z;
1648 
1649  start = clock();
1650  while (mip_filter_sensor2vehicle_tranformation(&device_interface_,
1651  MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK)
1652  {
1653  if (clock() - start > 5000)
1654  {
1655  ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out.");
1656  break;
1657  }
1658  }
1659 
1660  // Read back the transformation
1661  start = clock();
1662  while (mip_filter_sensor2vehicle_tranformation(&device_interface_,
1663  MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK)
1664  {
1665  if (clock() - start > 5000)
1666  {
1667  ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out.");
1668  break;
1669  }
1670  }
1671 
1672  if ((abs(readback_angles[0]-angles[0]) < 0.001) &&
1673  (abs(readback_angles[1]-angles[1]) < 0.001) &&
1674  (abs(readback_angles[2]-angles[2]) < 0.001))
1675  {
1676  ROS_INFO("Transformation successfully set.\n");
1677  ROS_INFO("New angles: %f roll %f pitch %f yaw\n",
1678  readback_angles[0], readback_angles[1], readback_angles[2]);
1679  }
1680  else
1681  {
1682  ROS_INFO("ERROR: Failed to set transformation!!!\n");
1683  ROS_INFO("Sent angles: %f roll %f pitch %f yaw\n",
1684  angles[0], angles[1], angles[2]);
1685  ROS_INFO("Returned angles: %f roll %f pitch %f yaw\n",
1686  readback_angles[0], readback_angles[1], readback_angles[2]);
1687  }
1688  res.success = true;
1689  return true;
1690  }
1691 
1692  // Get sensor to vehicle frame transformation
1693  bool Microstrain::get_sensor_vehicle_frame_trans(std_srvs::Trigger::Request &req,
1694  std_srvs::Trigger::Response &res)
1695  {
1696  if (GX5_15 == true)
1697  {
1698  ROS_INFO("Device does not support this feature");
1699  res.success = false;
1700  return true;
1701  }
1702 
1703  memset(readback_angles, 0, 3*sizeof(float));
1704  // Read back the transformation
1705  start = clock();
1706  while (mip_filter_sensor2vehicle_tranformation(&device_interface_,
1707  MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK)
1708  {
1709  if (clock() - start > 5000)
1710  {
1711  ROS_INFO("mip_filter_sensor2vehicle_tranformation function timed out.");
1712  break;
1713  }
1714  }
1715 
1716  ROS_INFO("Sensor Vehicle Frame Transformation Angles: %f roll %f pitch %f yaw\n",
1717  readback_angles[0], readback_angles[1], readback_angles[2]);
1718 
1719  res.success = true;
1720  return true;
1721  }
1722 
1723  // Set reference position
1724  bool Microstrain::set_reference_position(microstrain_mips::SetReferencePosition::Request &req,
1725  microstrain_mips::SetReferencePosition::Response &res)
1726  {
1727  ROS_INFO("Setting reference Position\n");
1728 
1729  memset(reference_position_command, 0, 3*sizeof(double));
1730  memset(reference_position_readback, 0, 3*sizeof(double));
1731  reference_position_enable_command = 1;
1732  reference_position_enable_readback = 1;
1733 
1734  reference_position_command[0] = req.position.x;
1735  reference_position_command[1] = req.position.y;
1736  reference_position_command[2] = req.position.z;
1737 
1738  start = clock();
1739  while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE,
1740  &reference_position_enable_command, reference_position_command) != MIP_INTERFACE_OK)
1741  {
1742  if (clock() - start > 5000)
1743  {
1744  ROS_INFO("mip_filter_reference_position function timed out.");
1745  break;
1746  }
1747  }
1748 
1749  // Read back the reference position
1750  start = clock();
1751  while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ,
1752  &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK)
1753  {
1754  if (clock() - start > 5000)
1755  {
1756  ROS_INFO("mip_filter_reference_position function timed out.");
1757  break;
1758  }
1759  }
1760 
1761  if ((reference_position_enable_command == reference_position_enable_readback) &&
1762  (abs(reference_position_command[0] - reference_position_readback[0]) < 0.001) &&
1763  (abs(reference_position_command[1] - reference_position_readback[1]) < 0.001) &&
1764  (abs(reference_position_command[2] - reference_position_readback[2]) < 0.001))
1765  {
1766  ROS_INFO("Reference position successfully set\n");
1767  }
1768  else
1769  {
1770  ROS_ERROR("Failed to set the reference position!!!\n");
1771  }
1772 
1773  res.success = true;
1774  return true;
1775  }
1776 
1777  // Get reference position
1778  bool Microstrain::get_reference_position(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
1779  {
1780  ROS_INFO("Getting reference position");
1781  memset(reference_position_readback, 0, 3*sizeof(double));
1782  start = clock();
1783  while (mip_filter_reference_position(&device_interface_, MIP_FUNCTION_SELECTOR_READ,
1784  &reference_position_enable_readback, reference_position_readback) != MIP_INTERFACE_OK)
1785  {
1786  if (clock() - start > 5000)
1787  {
1788  ROS_INFO("mip_filter_reference_position function timed out.");
1789  break;
1790  }
1791  }
1792  ROS_INFO("Reference position: Lat %f , Long %f, Alt %f", reference_position_readback[0],
1793  reference_position_readback[1], reference_position_readback[2]);
1794 
1795  res.success = true;
1796  return true;
1797  }
1798 
1799  // Enable or disable coning and sculling compensation
1800  bool Microstrain::set_coning_sculling_comp(microstrain_mips::SetConingScullingComp::Request &req,
1801  microstrain_mips::SetConingScullingComp::Response &res)
1802  {
1803  if (req.enable == 0)
1804  {
1805  ROS_INFO("Disabling Coning and Sculling compensation\n");
1806  enable_flag = MIP_3DM_CONING_AND_SCULLING_DISABLE;
1807  start = clock();
1808  while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_,
1809  MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK)
1810  {
1811  if (clock() - start > 5000)
1812  {
1813  ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out.");
1814  break;
1815  }
1816  }
1817 
1818  ROS_INFO("Reading Coning and Sculling compensation enabled state:\n");
1819  start = clock();
1820  while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_,
1821  MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK)
1822  {
1823  if (clock() - start > 5000)
1824  {
1825  ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out.");
1826  break;
1827  }
1828  }
1829  ROS_INFO("%s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
1830  }
1831  else if (req.enable == 1)
1832  {
1833  ROS_INFO("Enabling Coning and Sculling compensation\n");
1834  enable_flag = MIP_3DM_CONING_AND_SCULLING_ENABLE;
1835  start = clock();
1836  while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_,
1837  MIP_FUNCTION_SELECTOR_WRITE, &enable_flag) != MIP_INTERFACE_OK)
1838  {
1839  if (clock() - start > 5000)
1840  {
1841  ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out.");
1842  break;
1843  }
1844  }
1845 
1846  // Read back enable/disable value
1847  ROS_INFO("Reading Coning and Sculling compensation enabled state:\n");
1848  start = clock();
1849  while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_,
1850  MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK)
1851  {
1852  if (clock() - start > 5000)
1853  {
1854  ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out.");
1855  break;
1856  }
1857  }
1858  ROS_INFO("%s\n\n", enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
1859  }
1860  else
1861  {
1862  ROS_INFO("Error: Input must be either 0 (disable) or 1 (enable).");
1863  }
1864 
1865  res.success = false;
1866  return true;
1867  }
1868 
1869  // Get coning and sculling compenastion enabled/disabled state
1870  bool Microstrain::get_coning_sculling_comp(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
1871  {
1872  start = clock();
1873  while (mip_3dm_cmd_coning_sculling_compensation(&device_interface_,
1874  MIP_FUNCTION_SELECTOR_READ, &enable_flag) != MIP_INTERFACE_OK)
1875  {
1876  if (clock() - start > 5000)
1877  {
1878  ROS_INFO("mip_3dm_cmd_coning_sculling_compensation function timed out.");
1879  break;
1880  }
1881  }
1882  ROS_INFO("Coning and Sculling compensation is: %s\n\n",
1883  enable_flag == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
1884  res.success = true;
1885  return true;
1886  }
1887 
1888  // Set estimation control filter flags
1889  bool Microstrain::set_estimation_control_flags(microstrain_mips::SetEstimationControlFlags::Request &req,
1890  microstrain_mips::SetEstimationControlFlags::Response &res)
1891  {
1892  estimation_control = req.flag;
1893  start = clock();
1894  while (mip_filter_estimation_control(&device_interface_,
1895  MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK)
1896  {
1897  if (clock() - start > 5000)
1898  {
1899  ROS_INFO("mip_filter_estimation_control function timed out.");
1900  break;
1901  }
1902  }
1903 
1904  start = clock();
1905  while (mip_filter_estimation_control(&device_interface_,
1906  MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK)
1907  {
1908  if (clock() - start > 5000)
1909  {
1910  ROS_INFO("mip_filter_estimation_control function timed out.");
1911  break;
1912  }
1913  }
1914  ROS_INFO("Estimation control set to: %d", estimation_control_readback);
1915 
1916  res.success = true;
1917  return true;
1918  }
1919 
1920 
1921  // Get estimatio control filter flags
1922  bool Microstrain::get_estimation_control_flags(std_srvs::Trigger::Request &req,
1923  std_srvs::Trigger::Response &res)
1924  {
1925  start = clock();
1926  while (mip_filter_estimation_control(&device_interface_,
1927  MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK)
1928  {
1929  if (clock() - start > 5000)
1930  {
1931  ROS_INFO("mip_filter_estimation_control function timed out.");
1932  break;
1933  }
1934  }
1935  ROS_INFO("Estimation control set to: %d", estimation_control_readback);
1936 
1937  res.success = true;
1938  return true;
1939  }
1940 
1941 
1942  // Get device basic status. Variables in basic status struct change based on device model
1943  bool Microstrain::get_basic_status(std_srvs::Trigger::Request &req,
1944  std_srvs::Trigger::Response &res)
1945  {
1946  // Use the basic status struct for the GX-25
1947  if (GX5_25)
1948  {
1949  u8 response_buffer[sizeof(gx4_25_basic_status_field)];
1950  start = clock();
1951  while (mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER,
1952  GX4_25_BASIC_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK)
1953  {
1954  if (clock() - start > 5000)
1955  {
1956  ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out.");
1957  break;
1958  }
1959  }
1960 
1961  ROS_INFO("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model);
1962  ROS_INFO("Status Selector: \t\t\t\t%d\n", basic_field.status_selector);
1963  // == GX4_25_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
1964  ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", basic_field.status_flags);
1965  ROS_INFO("System state: \t\t\t\t\t%04u\n", basic_field.system_state);
1966  ROS_INFO("System Microsecond Timer Count: \t\t%lu ms\n\n", basic_field.system_timer_ms);
1967  }
1968  else
1969  {
1970  ROS_INFO("Command not supported on this model");
1971  }
1972 
1973  res.success = true;
1974  return true;
1975  }
1976 
1977  // Get diagnostic status of device. Changes based on device model.
1978  bool Microstrain::get_diagnostic_report(std_srvs::Trigger::Request &req,
1979  std_srvs::Trigger::Response &res)
1980  {
1981  // Use GX5-25 device diagnostic struct
1982  if (GX5_25)
1983  {
1984  u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)];
1985  start = clock();
1986  while (mip_3dm_cmd_hw_specific_device_status(&device_interface_, GX4_25_MODEL_NUMBER,
1987  GX4_25_DIAGNOSTICS_STATUS_SEL, response_buffer) != MIP_INTERFACE_OK)
1988  {
1989  if (clock() - start > 5000)
1990  {
1991  ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out.");
1992  break;
1993  }
1994  }
1995 
1996  ROS_INFO("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model);
1997  ROS_INFO("Status Selector: \t\t\t\t%d\n", diagnostic_field.status_selector);
1998  // == 114 ? "Basic Status Report" : "Diagnostic Status Report");
1999  ROS_INFO("Status Flags: \t\t\t\t\t%lu\n", diagnostic_field.status_flags);
2000  ROS_INFO("System Millisecond Timer Count: \t\t%lu ms\n", diagnostic_field.system_timer_ms);
2001  ROS_INFO("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE");
2002  ROS_INFO("FILTER Streaming Enabled: \t\t\t%s\n", diagnostic_field.filter_stream_enabled == 1 ? "TRUE" : "FALSE");
2003  ROS_INFO("Number of Dropped IMU Packets: \t\t\t%lu packets\n", diagnostic_field.imu_dropped_packets);
2004  ROS_INFO("Number of Dropped FILTER Packets: \t\t%lu packets\n", diagnostic_field.filter_dropped_packets);
2005  ROS_INFO("Communications Port Bytes Written: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_written);
2006  ROS_INFO("Communications Port Bytes Read: \t\t%lu Bytes\n", diagnostic_field.com1_port_bytes_read);
2007  ROS_INFO("Communications Port Write Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_write_overruns);
2008  ROS_INFO("Communications Port Read Overruns: \t\t%lu Bytes\n", diagnostic_field.com1_port_read_overruns);
2009  ROS_INFO("IMU Parser Errors: \t\t\t\t%lu Errors\n", diagnostic_field.imu_parser_errors);
2010  ROS_INFO("IMU Message Count: \t\t\t\t%lu Messages\n", diagnostic_field.imu_message_count);
2011  ROS_INFO("IMU Last Message Received: \t\t\t%lu ms\n", diagnostic_field.imu_last_message_ms);
2012  }
2013  // Unable to get this function working for the GX5-45
2014  else
2015  {
2016  ROS_INFO("Command not supported on this model");
2017  }
2018 
2019  res.success = true;
2020  return true;
2021  }
2022 
2023  // Set zero angular-rate update threshold
2024  bool Microstrain::set_zero_angle_update_threshold(microstrain_mips::SetZeroAngleUpdateThreshold::Request &req,
2025  microstrain_mips::SetZeroAngleUpdateThreshold::Response &res)
2026  {
2027  ROS_INFO("Setting Zero Angular-Rate-Update threshold\n");
2028 
2029  zero_update_control.threshold = req.threshold; // rads/s
2030  zero_update_control.enable = req.enable; // enable zero-angular-rate update
2031 
2032  // Set ZUPT parameters
2033  start = clock();
2034  while (mip_filter_zero_angular_rate_update_control(&device_interface_,
2035  MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK)
2036  {
2037  if (clock() - start > 5000)
2038  {
2039  ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out.");
2040  break;
2041  }
2042  }
2043 
2044  // Read back parameter settings
2045  start = clock();
2046  while (mip_filter_zero_angular_rate_update_control(&device_interface_,
2047  MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK)
2048  {
2049  if (clock() - start > 5000)
2050  {
2051  ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out.");
2052  break;
2053  }
2054  }
2055 
2056  if (zero_update_control.enable != zero_update_readback.enable ||
2057  zero_update_control.threshold != zero_update_readback.threshold)
2058  ROS_INFO("ERROR configuring Zero Angular Rate Update.\n");
2059 
2060  ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s",
2061  zero_update_readback.enable, zero_update_readback.threshold);
2062 
2063  res.success = true;
2064  return true;
2065  }
2066 
2067 
2068  // Get zero angular rate update threshold value
2069  bool Microstrain::get_zero_angle_update_threshold(std_srvs::Trigger::Request &req,
2070  std_srvs::Trigger::Response &res)
2071  {
2072  ROS_INFO("Setting Zero Angular-Rate-Update threshold\n");
2073  // Read back parameter settings
2074 
2075  start = clock();
2076  while (mip_filter_zero_angular_rate_update_control(&device_interface_,
2077  MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK)
2078  {
2079  if (clock() - start > 5000)
2080  {
2081  ROS_INFO("mip_filter_zero_angular_rate_update_control function timed out.");
2082  break;
2083  }
2084  }
2085  ROS_INFO("Enable value set to: %d, Threshold is: %f rad/s",
2086  zero_update_readback.enable, zero_update_readback.threshold);
2087 
2088  res.success = true;
2089  return true;
2090  }
2091 
2092  // Set tare orientation angle values
2093  bool Microstrain::set_tare_orientation(microstrain_mips::SetTareOrientation::Request &req,
2094  microstrain_mips::SetTareOrientation::Response &res)
2095  {
2096  if (req.axis < 1 || req.axis > 7)
2097  {
2098  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");
2099  res.success = false;
2100  }
2101 
2102  angles[0] = angles[1] = angles[2] = 0;
2103  int i = req.axis;
2104 
2105  start = clock();
2106  while (mip_filter_set_init_attitude(&device_interface_, angles) != MIP_INTERFACE_OK)
2107  {
2108  if (clock() - start > 5000)
2109  {
2110  ROS_INFO("mip_filter_set_init_attitude function timed out.");
2111  break;
2112  }
2113  }
2114 
2115  // Wait for Filter to re-establish running state
2116  Sleep(5000);
2117  // Cycle through axes combinations
2118  if (mip_filter_tare_orientation(&device_interface_,
2119  MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK)
2120  {
2121  ROS_INFO("ERROR: Failed Axis - ");
2122 
2123  if (i & FILTER_TARE_ROLL_AXIS)
2124  {
2125  ROS_INFO(" Roll Axis ");
2126  }
2127 
2128  if (i & FILTER_TARE_PITCH_AXIS)
2129  {
2130  ROS_INFO(" Pitch Axis ");
2131  }
2132 
2133  if (i & FILTER_TARE_YAW_AXIS)
2134  {
2135  ROS_INFO(" Yaw Axis ");
2136  }
2137  }
2138  else
2139  {
2140  ROS_INFO("Tare Configuration = %d\n", i);
2141 
2142  ROS_INFO("Tared -");
2143 
2144  if (i & FILTER_TARE_ROLL_AXIS)
2145  {
2146  ROS_INFO(" Roll Axis ");
2147  }
2148 
2149  if (i & FILTER_TARE_PITCH_AXIS)
2150  {
2151  ROS_INFO(" Pitch Axis ");
2152  }
2153 
2154  if (i & FILTER_TARE_YAW_AXIS)
2155  {
2156  ROS_INFO(" Yaw Axis ");
2157  }
2158 
2159  res.success = true;
2160  return true;
2161  }
2162 
2163  Sleep(1000);
2164  }
2165 
2166  // Set accel noise values
2167  bool Microstrain::set_accel_noise(microstrain_mips::SetAccelNoise::Request &req,
2168  microstrain_mips::SetAccelNoise::Response &res)
2169  {
2170  ROS_INFO("Setting the accel noise values\n");
2171 
2172  noise[0] = req.noise.x;
2173  noise[1] = req.noise.y;
2174  noise[2] = req.noise.z;
2175 
2176  start = clock();
2177  while (mip_filter_accel_noise(&device_interface_,
2178  MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK)
2179  {
2180  if (clock() - start > 5000)
2181  {
2182  ROS_INFO("mip_filter_accel_noise function timed out.");
2183  break;
2184  }
2185  }
2186 
2187  // Read back the accel noise values
2188  start = clock();
2189  while (mip_filter_accel_noise(&device_interface_,
2190  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2191  {
2192  if (clock() - start > 5000)
2193  {
2194  ROS_INFO("mip_filter_accel_noise function timed out.");
2195  break;
2196  }
2197  }
2198 
2199  if ((abs(readback_noise[0]-noise[0]) < 0.001) &&
2200  (abs(readback_noise[1]-noise[1]) < 0.001) &&
2201  (abs(readback_noise[2]-noise[2]) < 0.001))
2202  {
2203  ROS_INFO("Accel noise values successfully set.\n");
2204  }
2205  else
2206  {
2207  ROS_INFO("ERROR: Failed to set accel noise values!!!\n");
2208  ROS_INFO("Sent values: %f X %f Y %f Z\n",
2209  noise[0], noise[1], noise[2]);
2210  ROS_INFO("Returned values: %f X %f Y %f Z\n",
2211  readback_noise[0], readback_noise[1], readback_noise[2]);
2212  }
2213 
2214  res.success;
2215  return true;
2216  }
2217 
2218  // Get accel noise values
2219  bool Microstrain::get_accel_noise(std_srvs::Trigger::Request &req,
2220  std_srvs::Trigger::Response &res)
2221  {
2222  start = clock();
2223  while (mip_filter_accel_noise(&device_interface_,
2224  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2225  {
2226  if (clock() - start > 5000)
2227  {
2228  ROS_INFO("mip_filter_accel_noise function timed out.");
2229  break;
2230  }
2231  }
2232  ROS_INFO("Accel noise values: %f X %f Y %f Z\n",
2233  readback_noise[0], readback_noise[1], readback_noise[2]);
2234 
2235  res.success = true;
2236  return true;
2237  }
2238 
2239  // Set gyro noise values
2240  bool Microstrain::set_gyro_noise(microstrain_mips::SetGyroNoise::Request &req,
2241  microstrain_mips::SetGyroNoise::Response &res)
2242  {
2243  ROS_INFO("Setting the gyro noise values\n");
2244 
2245  noise[0] = req.noise.x;
2246  noise[1] = req.noise.y;
2247  noise[2] = req.noise.z;
2248 
2249  start = clock();
2250  while (mip_filter_gyro_noise(&device_interface_,
2251  MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK)
2252  {
2253  if (clock() - start > 5000)
2254  {
2255  ROS_INFO("mip_filter_gyro_noise function timed out.");
2256  break;
2257  }
2258  }
2259 
2260  // Read back the gyro noise values
2261  start = clock();
2262  while (mip_filter_gyro_noise(&device_interface_,
2263  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2264  {
2265  if (clock() - start > 5000)
2266  {
2267  ROS_INFO("mip_filter_gyro_noise function timed out.");
2268  break;
2269  }
2270  }
2271 
2272  if ((abs(readback_noise[0]-noise[0]) < 0.001) &&
2273  (abs(readback_noise[1]-noise[1]) < 0.001) &&
2274  (abs(readback_noise[2]-noise[2]) < 0.001))
2275  {
2276  ROS_INFO("Gyro noise values successfully set.\n");
2277  }
2278  else
2279  {
2280  ROS_INFO("ERROR: Failed to set gyro noise values!!!\n");
2281  ROS_INFO("Sent values: %f X %f Y %f Z\n",
2282  noise[0], noise[1], noise[2]);
2283  ROS_INFO("Returned values: %f X %f Y %f Z\n",
2284  readback_noise[0], readback_noise[1], readback_noise[2]);
2285  }
2286 
2287  res.success = true;
2288  return true;
2289  }
2290 
2291  // Get gyro noise values
2292  bool Microstrain::get_gyro_noise(std_srvs::Trigger::Request &req,
2293  std_srvs::Trigger::Response &res)
2294  {
2295  start = clock();
2296  while (mip_filter_gyro_noise(&device_interface_,
2297  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2298  {
2299  if (clock() - start > 5000)
2300  {
2301  ROS_INFO("mip_filter_gyro_noise function timed out.");
2302  break;
2303  }
2304  }
2305  ROS_INFO("Gyro noise values: %f X %f Y %f Z\n",
2306  readback_noise[0], readback_noise[1], readback_noise[2]);
2307 
2308  res.success = true;
2309  return true;
2310  }
2311 
2312  // Set magnetometer noise values
2313  bool Microstrain::set_mag_noise(microstrain_mips::SetMagNoise::Request &req,
2314  microstrain_mips::SetMagNoise::Response &res)
2315  {
2316  if (GX5_15 == true)
2317  {
2318  ROS_INFO("Device does not support this feature");
2319  res.success = false;
2320  return true;
2321  }
2322 
2323  ROS_INFO("Setting the mag noise values\n");
2324 
2325  noise[0] = req.noise.x;
2326  noise[1] = req.noise.y;
2327  noise[2] = req.noise.z;
2328 
2329  start = clock();
2330  while (mip_filter_mag_noise(&device_interface_,
2331  MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK)
2332  {
2333  if (clock() - start > 5000)
2334  {
2335  ROS_INFO("mip_filter_mag_noise function timed out.");
2336  break;
2337  }
2338  }
2339 
2340  // Read back the mag white noise values
2341  start = clock();
2342  while (mip_filter_mag_noise(&device_interface_,
2343  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2344  {
2345  if (clock() - start > 5000)
2346  {
2347  ROS_INFO("mip_filter_mag_noise function timed out.");
2348  break;
2349  }
2350  }
2351 
2352  if ((abs(readback_noise[0] - noise[0]) < 0.001) &&
2353  (abs(readback_noise[1] - noise[1]) < 0.001) &&
2354  (abs(readback_noise[2] - noise[2]) < 0.001))
2355  {
2356  ROS_INFO("Mag noise values successfully set.\n");
2357  }
2358  else
2359  {
2360  ROS_INFO("ERROR: Failed to set mag noise values!!!\n");
2361  ROS_INFO("Sent values: %f X %f Y %f Z\n",
2362  noise[0], noise[1], noise[2]);
2363  ROS_INFO("Returned values: %f X %f Y %f Z\n",
2364  readback_noise[0], readback_noise[1], readback_noise[2]);
2365  }
2366 
2367  res.success = true;
2368  return true;
2369  }
2370 
2371  // Get magnetometer noise values
2372  bool Microstrain::get_mag_noise(std_srvs::Trigger::Request &req,
2373  std_srvs::Trigger::Response &res)
2374  {
2375  if (GX5_15 == true)
2376  {
2377  ROS_INFO("Device does not support this feature");
2378  res.success = false;
2379  return true;
2380  }
2381 
2382  start = clock();
2383  while (mip_filter_mag_noise(&device_interface_,
2384  MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK)
2385  {
2386  if (clock() - start > 5000)
2387  {
2388  ROS_INFO("mip_filter_mag_noise function timed out.");
2389  break;
2390  }
2391  }
2392  ROS_INFO("Returned values: %f X %f Y %f Z\n",
2393  readback_noise[0], readback_noise[1], readback_noise[2]);
2394 
2395  res.success = true;
2396  return true;
2397  }
2398 
2399 
2400  // Set gyro bias model
2401  bool Microstrain::set_gyro_bias_model(microstrain_mips::SetGyroBiasModel::Request &req,
2402  microstrain_mips::SetGyroBiasModel::Response &res)
2403  {
2404  ROS_INFO("Setting the gyro bias model values\n");
2405 
2406  noise[0] = req.noise_vector.x;
2407  noise[1] = req.noise_vector.y;
2408  noise[2] = req.noise_vector.z;
2409 
2410  beta[0] = req.beta_vector.x;
2411  beta[1] = req.beta_vector.x;
2412  beta[2] = req.beta_vector.x;
2413 
2414  start = clock();
2415  while (mip_filter_gyro_bias_model(&device_interface_,
2416  MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK)
2417  {
2418  if (clock() - start > 5000)
2419  {
2420  ROS_INFO("mip_filter_gyro_bias_model function timed out.");
2421  break;
2422  }
2423  }
2424 
2425  // Read back the gyro bias model values
2426  start = clock();
2427  while (mip_filter_gyro_bias_model(&device_interface_,
2428  MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK)
2429  {
2430  if (clock() - start > 5000)
2431  {
2432  ROS_INFO("mip_filter_gyro_bias_model function timed out.");
2433  break;
2434  }
2435  }
2436 
2437  if ((abs(readback_noise[0]-noise[0]) < 0.001) &&
2438  (abs(readback_noise[1]-noise[1]) < 0.001) &&
2439  (abs(readback_noise[2]-noise[2]) < 0.001) &&
2440  (abs(readback_beta[0]-beta[0]) < 0.001) &&
2441  (abs(readback_beta[1]-beta[1]) < 0.001) &&
2442  (abs(readback_beta[2]-beta[2]) < 0.001))
2443  {
2444  ROS_INFO("Gyro bias model values successfully set.\n");
2445  }
2446  else
2447  {
2448  ROS_INFO("ERROR: Failed to set gyro bias model values!!!\n");
2449  ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2450  beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]);
2451  ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2452  readback_beta[0], readback_beta[1], readback_beta[2],
2453  readback_noise[0], readback_noise[1], readback_noise[2]);
2454  }
2455 
2456  res.success = true;
2457  return true;
2458  }
2459 
2460  // Get gyro bias model
2461  bool Microstrain::get_gyro_bias_model(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
2462  {
2463  start = clock();
2464  while (mip_filter_gyro_bias_model(&device_interface_,
2465  MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK)
2466  {
2467  if (clock() - start > 5000)
2468  {
2469  ROS_INFO("mip_filter_gyro_bias_model function timed out.");
2470  break;
2471  }
2472  }
2473 
2474  ROS_INFO("Gyro bias model values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2475  readback_beta[0], readback_beta[1], readback_beta[2], readback_noise[0], readback_noise[1], readback_noise[2]);
2476  res.success = true;
2477  return true;
2478  }
2479 
2480  // Get acces bias model
2481  bool Microstrain::get_accel_bias_model(std_srvs::Trigger::Request &req,
2482  std_srvs::Trigger::Response &res)
2483  {
2484  if (GX5_15 == true || GX5_25 == true)
2485  {
2486  ROS_INFO("Device does not support this feature");
2487  res.success = false;
2488  return true;
2489  }
2490 
2491  memset(readback_noise, 0, 3*sizeof(float));
2492  memset(readback_beta, 0, 3*sizeof(float));
2493 
2494  start = clock();
2495  while (mip_filter_accel_bias_model(&device_interface_,
2496  MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK)
2497  {
2498  if (clock() - start > 5000)
2499  {
2500  ROS_INFO("mip_filter_accel_bias_model function timed out.");
2501  break;
2502  }
2503  }
2504 
2505  ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2506  readback_beta[0], readback_beta[1], readback_beta[2],
2507  readback_noise[0], readback_noise[1], readback_noise[2]);
2508 
2509  res.success = true;
2510  return true;
2511  }
2512 
2513  // Set accel bias model
2514  bool Microstrain::set_accel_bias_model(microstrain_mips::SetAccelBiasModel::Request &req,
2515  microstrain_mips::SetAccelBiasModel::Response &res)
2516  {
2517  if (GX5_15 == true || GX5_25 == true)
2518  {
2519  ROS_INFO("Device does not support this feature");
2520  res.success = false;
2521  return true;
2522  }
2523 
2524  memset(noise, 0, 3*sizeof(float));
2525  memset(beta, 0, 3*sizeof(float));
2526  memset(readback_noise, 0, 3*sizeof(float));
2527  memset(readback_beta, 0, 3*sizeof(float));
2528  ROS_INFO("Setting the accel bias model values\n");
2529 
2530  noise[0] = req.noise_vector.x;
2531  noise[1] = req.noise_vector.y;
2532  noise[2] = req.noise_vector.z;
2533 
2534  beta[0] = req.beta_vector.x;
2535  beta[1] = req.beta_vector.x;
2536  beta[2] = req.beta_vector.x;
2537 
2538  start = clock();
2539  while (mip_filter_accel_bias_model(&device_interface_,
2540  MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK)
2541  {
2542  if (clock() - start > 5000)
2543  {
2544  ROS_INFO("mip_filter_accel_bias_model function timed out.");
2545  break;
2546  }
2547  }
2548 
2549  // Read back the accel bias model values
2550  start = clock();
2551  while (mip_filter_accel_bias_model(&device_interface_, MIP_FUNCTION_SELECTOR_READ,
2552  readback_beta, readback_noise) != MIP_INTERFACE_OK)
2553  {
2554  if (clock() - start > 5000)
2555  {
2556  ROS_INFO("mip_filter_accel_bias_model function timed out.");
2557  break;
2558  }
2559  }
2560 
2561  if ((abs(readback_noise[0]-noise[0]) < 0.001) &&
2562  (abs(readback_noise[1]-noise[1]) < 0.001) &&
2563  (abs(readback_noise[2]-noise[2]) < 0.001) &&
2564  (abs(readback_beta[0]-beta[0]) < 0.001) &&
2565  (abs(readback_beta[1]-beta[1]) < 0.001) &&
2566  (abs(readback_beta[2]-beta[2]) < 0.001))
2567  {
2568  ROS_INFO("Accel bias model values successfully set.\n");
2569  }
2570  else
2571  {
2572  ROS_INFO("ERROR: Failed to set accel bias model values!!!\n");
2573  ROS_INFO("Sent values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2574  beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]);
2575  ROS_INFO("Returned values: Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n",
2576  readback_beta[0], readback_beta[1], readback_beta[2],
2577  readback_noise[0], readback_noise[1], readback_noise[2]);
2578  }
2579 
2580  res.success = true;
2581  return true;
2582  }
2583 
2584  // Set accel magnitude error adaptive measurement values
2585  bool Microstrain::set_accel_adaptive_vals(microstrain_mips::SetAccelAdaptiveVals::Request &req,
2586  microstrain_mips::SetAccelAdaptiveVals::Response &res )
2587  {
2588  ROS_INFO("Setting the accel magnitude error adaptive measurement values\n");
2589 
2590  accel_magnitude_error_command.enable = req.enable;
2591  accel_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff;
2592  accel_magnitude_error_command.min_1sigma = req.min_1sigma;
2593  accel_magnitude_error_command.low_limit = req.low_limit;
2594  accel_magnitude_error_command.high_limit = req.high_limit;
2595  accel_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma;
2596  accel_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma;
2597 
2598  start = clock();
2599  while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_,
2600  MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK)
2601  {
2602  if (clock() - start > 5000)
2603  {
2604  ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out.");
2605  break;
2606  }
2607  }
2608 
2609  // Read back the accel magnitude error adaptive measurement values
2610  start = clock();
2611  while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_,
2612  MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK)
2613  {
2614  if (clock() - start > 5000)
2615  {
2616  ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out.");
2617  break;
2618  }
2619  }
2620 
2621  if ((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) &&
2622  (abs(accel_magnitude_error_command.low_pass_cutoff - accel_magnitude_error_readback.low_pass_cutoff) < 0.001) &&
2623  (abs(accel_magnitude_error_command.min_1sigma - accel_magnitude_error_readback.min_1sigma) < 0.001) &&
2624  (abs(accel_magnitude_error_command.low_limit - accel_magnitude_error_readback.low_limit) < 0.001) &&
2625  (abs(accel_magnitude_error_command.high_limit - accel_magnitude_error_readback.high_limit) < 0.001) &&
2626  (abs(accel_magnitude_error_command.low_limit_1sigma - accel_magnitude_error_readback.low_limit_1sigma) < 0.001) &&
2627  (abs(accel_magnitude_error_command.high_limit_1sigma - accel_magnitude_error_readback.high_limit_1sigma) < 0.001))
2628  {
2629  ROS_INFO("accel magnitude error adaptive measurement values successfully set.\n");
2630  }
2631  else
2632  {
2633  ROS_INFO("ERROR: Failed to set accel magnitude error adaptive measurement values!!!");
2634  ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f",
2635  accel_magnitude_error_command.enable, accel_magnitude_error_command.low_pass_cutoff,
2636  accel_magnitude_error_command.min_1sigma, accel_magnitude_error_command.low_limit,
2637  accel_magnitude_error_command.high_limit, accel_magnitude_error_command.low_limit_1sigma,
2638  accel_magnitude_error_command.high_limit_1sigma);
2639  ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f",
2640  accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff,
2641  accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit,
2642  accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma,
2643  accel_magnitude_error_readback.high_limit_1sigma);
2644  }
2645 
2646  res.success = true;
2647  return true;
2648  }
2649 
2650  // Get accep magnitude error adaptive measurement values
2651  bool Microstrain::get_accel_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res )
2652  {
2653  start = clock();
2654  while (mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface_,
2655  MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK)
2656  {
2657  if (clock() - start > 5000)
2658  {
2659  ROS_INFO("mip_filter_accel_magnitude_error_adaptive_measurement function timed out.");
2660  break;
2661  }
2662  }
2663  ROS_INFO("Accel magnitude error adaptive measurement values are: Enable: %i, Parameters: %f %f %f %f %f %f",
2664  accel_magnitude_error_readback.enable, accel_magnitude_error_readback.low_pass_cutoff,
2665  accel_magnitude_error_readback.min_1sigma, accel_magnitude_error_readback.low_limit,
2666  accel_magnitude_error_readback.high_limit, accel_magnitude_error_readback.low_limit_1sigma,
2667  accel_magnitude_error_readback.high_limit_1sigma);
2668 
2669  res.success = true;
2670  return true;
2671  }
2672 
2673  // Set magnetometer magnitude error adaptive measurement values
2674  bool Microstrain::set_mag_adaptive_vals(microstrain_mips::SetMagAdaptiveVals::Request &req,
2675  microstrain_mips::SetMagAdaptiveVals::Response &res )
2676  {
2677  if (GX5_15 == true)
2678  {
2679  ROS_INFO("Device does not support this feature");
2680  res.success = false;
2681  return true;
2682  }
2683 
2684  ROS_INFO("Setting the mag magnitude error adaptive measurement values\n");
2685 
2686  mag_magnitude_error_command.enable = req.enable;
2687  mag_magnitude_error_command.low_pass_cutoff = req.low_pass_cutoff;
2688  mag_magnitude_error_command.min_1sigma = req.min_1sigma;
2689  mag_magnitude_error_command.low_limit = req.low_limit;
2690  mag_magnitude_error_command.high_limit = req.high_limit;
2691  mag_magnitude_error_command.low_limit_1sigma = req.low_limit_1sigma;
2692  mag_magnitude_error_command.high_limit_1sigma = req.high_limit_1sigma;
2693 
2694  start = clock();
2695  while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_, MIP_FUNCTION_SELECTOR_WRITE,
2696  &mag_magnitude_error_command) != MIP_INTERFACE_OK)
2697  {
2698  if (clock() - start > 5000)
2699  {
2700  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2701  break;
2702  }
2703  }
2704 
2705  // Read back the mag magnitude error adaptive measurement values
2706  start = clock();
2707  while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_,
2708  MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK)
2709  {
2710  if (clock() - start > 5000)
2711  {
2712  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2713  break;
2714  }
2715  }
2716 
2717  if ((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) &&
2718  (abs(mag_magnitude_error_command.low_pass_cutoff - mag_magnitude_error_readback.low_pass_cutoff) < 0.001) &&
2719  (abs(mag_magnitude_error_command.min_1sigma - mag_magnitude_error_readback.min_1sigma) < 0.001) &&
2720  (abs(mag_magnitude_error_command.low_limit - mag_magnitude_error_readback.low_limit) < 0.001) &&
2721  (abs(mag_magnitude_error_command.high_limit - mag_magnitude_error_readback.high_limit) < 0.001) &&
2722  (abs(mag_magnitude_error_command.low_limit_1sigma - mag_magnitude_error_readback.low_limit_1sigma) < 0.001) &&
2723  (abs(mag_magnitude_error_command.high_limit_1sigma - mag_magnitude_error_readback.high_limit_1sigma) < 0.001))
2724  {
2725  ROS_INFO("mag magnitude error adaptive measurement values successfully set.\n");
2726  }
2727  else
2728  {
2729  ROS_INFO("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n");
2730  ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n",
2731  mag_magnitude_error_command.enable, mag_magnitude_error_command.low_pass_cutoff,
2732  mag_magnitude_error_command.min_1sigma, mag_magnitude_error_command.low_limit,
2733  mag_magnitude_error_command.high_limit, mag_magnitude_error_command.low_limit_1sigma,
2734  mag_magnitude_error_command.high_limit_1sigma);
2735  ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n",
2736  mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff,
2737  mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit,
2738  mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma,
2739  mag_magnitude_error_readback.high_limit_1sigma);
2740  }
2741 
2742  res.success = true;
2743  return true;
2744  }
2745 
2746  // Get magnetometer magnitude error adaptive measurement values
2747  bool Microstrain::get_mag_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res )
2748  {
2749  if (GX5_15 == true)
2750  {
2751  ROS_INFO("Device does not support this feature");
2752  res.success = false;
2753  return true;
2754  }
2755 
2756  start = clock();
2757  while (mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface_,
2758  MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK)
2759  {
2760  if (clock() - start > 5000)
2761  {
2762  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2763  break;
2764  }
2765  }
2766 
2767  ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n",
2768  mag_magnitude_error_readback.enable, mag_magnitude_error_readback.low_pass_cutoff,
2769  mag_magnitude_error_readback.min_1sigma, mag_magnitude_error_readback.low_limit,
2770  mag_magnitude_error_readback.high_limit, mag_magnitude_error_readback.low_limit_1sigma,
2771  mag_magnitude_error_readback.high_limit_1sigma);
2772  res.success = true;
2773  return true;
2774  }
2775 
2776  // Get magnetometer dip angle error adaptive measurement values
2777  bool Microstrain::get_mag_dip_adaptive_vals(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res )
2778  {
2779  if (GX5_15 == true || GX5_25 == true)
2780  {
2781  ROS_INFO("Device does not support this feature");
2782  res.success = false;
2783  return true;
2784  }
2785 
2786  start = clock();
2787  while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_,
2788  MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK)
2789  {
2790  if (clock() - start > 5000)
2791  {
2792  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2793  break;
2794  }
2795  }
2796 
2797  ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable,
2798  mag_dip_angle_error_readback.low_pass_cutoff,
2799  mag_dip_angle_error_readback.min_1sigma,
2800  mag_dip_angle_error_readback.high_limit,
2801  mag_dip_angle_error_readback.high_limit_1sigma);
2802 
2803  res.success = true;
2804  return true;
2805  }
2806 
2807  // Get magnetometer dip angle error adaptive measurement values
2808  bool Microstrain::set_mag_dip_adaptive_vals(microstrain_mips::SetMagDipAdaptiveVals::Request &req,
2809  microstrain_mips::SetMagDipAdaptiveVals::Response &res )
2810  {
2811  if (GX5_15 == true || GX5_25 == true)
2812  {
2813  ROS_INFO("Device does not support this feature");
2814  res.success = false;
2815  return true;
2816  }
2817 
2818  ROS_INFO("Setting the mag dip angle error adaptive measurement values\n");
2819 
2820  mag_dip_angle_error_command.enable = req.enable;
2821  mag_dip_angle_error_command.low_pass_cutoff = req.low_pass_cutoff;
2822  mag_dip_angle_error_command.min_1sigma = req.min_1sigma;
2823  mag_dip_angle_error_command.high_limit = req.high_limit;
2824  mag_dip_angle_error_command.high_limit_1sigma = req.high_limit_1sigma;
2825 
2826  start = clock();
2827  while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_,
2828  MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK)
2829  {
2830  if (clock() - start > 5000)
2831  {
2832  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2833  break;
2834  }
2835  }
2836 
2837  // Read back the mag magnitude error adaptive measurement values
2838  start = clock();
2839  while (mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface_,
2840  MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK)
2841  {
2842  if (clock() - start > 5000)
2843  {
2844  ROS_INFO("mip_filter_mag_magnitude_error_adaptive_measurement function timed out.");
2845  break;
2846  }
2847  }
2848 
2849  if ((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) &&
2850  (abs(mag_dip_angle_error_command.low_pass_cutoff - mag_dip_angle_error_readback.low_pass_cutoff) < 0.001) &&
2851  (abs(mag_dip_angle_error_command.min_1sigma - mag_dip_angle_error_readback.min_1sigma) < 0.001) &&
2852  (abs(mag_dip_angle_error_command.high_limit - mag_dip_angle_error_readback.high_limit) < 0.001) &&
2853  (abs(mag_dip_angle_error_command.high_limit_1sigma - mag_dip_angle_error_readback.high_limit_1sigma) < 0.001))
2854  {
2855  ROS_INFO("mag dip angle error adaptive measurement values successfully set.\n");
2856  }
2857  else
2858  {
2859  ROS_INFO("ERROR: Failed to set mag dip angle error adaptive measurement values!!!\n");
2860  ROS_INFO("Sent values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_command.enable,
2861  mag_dip_angle_error_command.low_pass_cutoff,
2862  mag_dip_angle_error_command.min_1sigma,
2863  mag_dip_angle_error_command.high_limit,
2864  mag_dip_angle_error_command.high_limit_1sigma);
2865 
2866  ROS_INFO("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable,
2867  mag_dip_angle_error_readback.low_pass_cutoff,
2868  mag_dip_angle_error_readback.min_1sigma,
2869  mag_dip_angle_error_readback.high_limit,
2870  mag_dip_angle_error_readback.high_limit_1sigma);
2871  }
2872 
2873  res.success = true;
2874  return true;
2875  }
2876 
2877  // Get vehicle dynamics mode
2878  bool Microstrain::get_dynamics_mode(std_srvs::Trigger::Request &req,
2879  std_srvs::Trigger::Response &res)
2880  {
2881  if (GX5_15 == true || GX5_25 == true)
2882  {
2883  ROS_INFO("Device does not support this feature");
2884  res.success = false;
2885  return true;
2886  }
2887 
2888  readback_dynamics_mode = 0;
2889  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
2890  MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) {}
2891 
2892  ROS_INFO("Vehicle dynamics mode is: %d\n", dynamics_mode);
2893 
2894  res.success = true;
2895  return true;
2896  }
2897 
2898 
2899  // Set vehicle dynamics mode. Only in 45 model.
2900  bool Microstrain::set_dynamics_mode(microstrain_mips::SetDynamicsMode::Request &req,
2901  microstrain_mips::SetDynamicsMode::Response &res)
2902  {
2903  if (GX5_15 == true || GX5_25 == true)
2904  {
2905  ROS_INFO("Device does not support this feature");
2906  res.success = false;
2907  return true;
2908  }
2909 
2910  dynamics_mode = req.mode;
2911 
2912  if (dynamics_mode < 1 || dynamics_mode > 3)
2913  {
2914  ROS_INFO("Error: Vehicle dynamics mode must be between 1-3");
2915  res.success = false;
2916  }
2917  else
2918  {
2919  start = clock();
2920  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
2921  MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK)
2922  {
2923  if (clock() - start > 5000)
2924  {
2925  ROS_INFO("mip_filter_vehicle_dynamics_mode function timed out.");
2926  break;
2927  }
2928  }
2929 
2930  readback_dynamics_mode = 0;
2931  while (mip_filter_vehicle_dynamics_mode(&device_interface_,
2932  MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK) {}
2933 
2934  if (dynamics_mode == readback_dynamics_mode)
2935  {
2936  ROS_INFO("Vehicle dynamics mode successfully set to %d\n", dynamics_mode);
2937  res.success = true;
2938  }
2939  else
2940  {
2941  ROS_INFO("ERROR: Failed to set vehicle dynamics mode to %d!!!\n", dynamics_mode);
2942  res.success = false;
2943  }
2944  }
2945  return true;
2946  }
2947 
2948  // Set sensor to vehicle frame offset. Only in 45
2949  bool Microstrain::set_sensor_vehicle_frame_offset(microstrain_mips::SetSensorVehicleFrameOffset::Request &req,
2950  microstrain_mips::SetSensorVehicleFrameOffset::Response &res)
2951  {
2952  if (GX5_15 == true || GX5_25 == true)
2953  {
2954  ROS_INFO("Device does not support this feature");
2955  res.success = false;
2956  return true;
2957  }
2958 
2959  memset(offset, 0, 3*sizeof(float));
2960  memset(readback_offset, 0, 3*sizeof(float));
2961  ROS_INFO("Setting the sensor to vehicle frame offset\n");
2962 
2963  offset[0] = req.offset.x;
2964  offset[1] = req.offset.y;
2965  offset[2] = req.offset.z;
2966 
2967  start = clock();
2968  while (mip_filter_sensor2vehicle_offset(&device_interface_,
2969  MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK)
2970  {
2971  if (clock() - start > 5000)
2972  {
2973  ROS_INFO("mip_filter_sensor2vehicle_offset function timed out.");
2974  break;
2975  }
2976  }
2977 
2978  // Read back the transformation
2979  start = clock();
2980  while (mip_filter_sensor2vehicle_offset(&device_interface_,
2981  MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK)
2982  {
2983  if (clock() - start > 5000)
2984  {
2985  ROS_INFO("mip_filter_sensor2vehicle_offset function timed out.");
2986  break;
2987  }
2988  }
2989 
2990  if ((abs(readback_offset[0]-offset[0]) < 0.001) &&
2991  (abs(readback_offset[1]-offset[1]) < 0.001) &&
2992  (abs(readback_offset[2]-offset[2]) < 0.001))
2993  {
2994  ROS_INFO("Offset successfully set.\n");
2995  }
2996  else
2997  {
2998  ROS_INFO("ERROR: Failed to set offset!!!\n");
2999  ROS_INFO("Sent offset: %f X %f Y %f Z\n", offset[0], offset[1], offset[2]);
3000  ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
3001  }
3002 
3003  res.success = true;
3004  return true;
3005  }
3006 
3007  // Get sensor to vehicle frame offset. Only in 45 model.
3008  bool Microstrain::get_sensor_vehicle_frame_offset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
3009  {
3010  if (GX5_15 == true || GX5_25 == true)
3011  {
3012  ROS_INFO("Device does not support this feature");
3013  res.success = false;
3014  return true;
3015  }
3016 
3017  memset(readback_offset, 0, 3*sizeof(float));
3018 
3019  start = clock();
3020  while (mip_filter_sensor2vehicle_offset(&device_interface_,
3021  MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK)
3022  {
3023  if (clock() - start > 5000)
3024  {
3025  ROS_INFO("mip_filter_sensor2vehicle_offset function timed out.");
3026  break;
3027  }
3028  }
3029 
3030  ROS_INFO("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
3031 
3032  res.success = true;
3033  return true;
3034  }
3035 
3036  // Get basic or diagnostic status of device. Called by basic and diagnostic services.
3037  u16 Microstrain::mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface,
3038  u16 model_number, u8 status_selector, u8 *response_buffer)
3039  {
3040  int total_size = 0;
3041  if (GX5_25)
3042  {
3043  gx4_25_basic_status_field *basic_ptr;
3044  gx4_25_diagnostic_device_status_field *diagnostic_ptr;
3045  u16 response_size = MIP_FIELD_HEADER_SIZE;
3046 
3047  // Set response size based on device model and whether basic or diagnostic status is chosen
3048  if (status_selector == GX4_25_BASIC_STATUS_SEL)
3049  {
3050  response_size += sizeof(gx4_25_basic_status_field);
3051  }
3052  else if (status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL)
3053  {
3054  response_size += sizeof(gx4_25_diagnostic_device_status_field);
3055  }
3056 
3057  while (mip_3dm_cmd_device_status(device_interface, model_number,
3058  status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK) {}
3059 
3060  if (status_selector == GX4_25_BASIC_STATUS_SEL)
3061  {
3062  if (response_size != sizeof(gx4_25_basic_status_field))
3063  {
3064  return MIP_INTERFACE_ERROR;
3065  }
3066  else if (MIP_SDK_CONFIG_BYTESWAP)
3067  {
3068  // Perform byteswapping
3069  byteswap_inplace(&response_buffer[0], sizeof(basic_field.device_model));
3070  byteswap_inplace(&response_buffer[2], sizeof(basic_field.status_selector));
3071  byteswap_inplace(&response_buffer[3], sizeof(basic_field.status_flags));
3072  byteswap_inplace(&response_buffer[7], sizeof(basic_field.system_state));
3073  byteswap_inplace(&response_buffer[9], sizeof(basic_field.system_timer_ms));
3074  }
3075 
3076  void * struct_pointer;
3077  struct_pointer = &basic_field;
3078 
3079  // Copy response from response buffer to basic status struct
3080  memcpy(struct_pointer, response_buffer, sizeof(basic_field.device_model));
3081  memcpy((struct_pointer+2), &(response_buffer[2]), sizeof(basic_field.status_selector));
3082  memcpy((struct_pointer+3), &(response_buffer[3]), sizeof(basic_field.status_flags));
3083  memcpy((struct_pointer+7), &(response_buffer[7]), sizeof(basic_field.system_state));
3084  memcpy((struct_pointer+9), &(response_buffer[9]), sizeof(basic_field.system_timer_ms));
3085  }
3086  else if (status_selector == GX4_25_DIAGNOSTICS_STATUS_SEL)
3087  {
3088  if (response_size != sizeof(gx4_25_diagnostic_device_status_field))
3089  {
3090  return MIP_INTERFACE_ERROR;
3091  }
3092  else if (MIP_SDK_CONFIG_BYTESWAP)
3093  {
3094  // byteswap and copy response to diagnostic status struct
3095  byteswap_inplace(&response_buffer[total_size],
3096  sizeof(diagnostic_field.device_model));
3097  total_size += sizeof(diagnostic_field.device_model);
3098  byteswap_inplace(&response_buffer[total_size],
3099  sizeof(diagnostic_field.status_selector));
3100  total_size += sizeof(diagnostic_field.status_selector);
3101  byteswap_inplace(&response_buffer[total_size],
3102  sizeof(diagnostic_field.status_flags));
3103  total_size += sizeof(diagnostic_field.status_flags);
3104  byteswap_inplace(&response_buffer[total_size],
3105  sizeof(diagnostic_field.system_state));
3106  total_size += sizeof(diagnostic_field.system_state);
3107  byteswap_inplace(&response_buffer[total_size],
3108  sizeof(diagnostic_field.system_timer_ms));
3109  total_size += sizeof(diagnostic_field.system_timer_ms);
3110  byteswap_inplace(&response_buffer[total_size],
3111  sizeof(diagnostic_field.imu_stream_enabled));
3112  total_size += sizeof(diagnostic_field.imu_stream_enabled);
3113  byteswap_inplace(&response_buffer[total_size],
3114  sizeof(diagnostic_field.filter_stream_enabled));
3115  total_size += sizeof(diagnostic_field.filter_stream_enabled);
3116  byteswap_inplace(&response_buffer[total_size],
3117  sizeof(diagnostic_field.imu_dropped_packets));
3118  total_size += sizeof(diagnostic_field.imu_dropped_packets);
3119  byteswap_inplace(&response_buffer[total_size],
3120  sizeof(diagnostic_field.filter_dropped_packets));
3121  total_size += sizeof(diagnostic_field.filter_dropped_packets);
3122  byteswap_inplace(&response_buffer[total_size],
3123  sizeof(diagnostic_field.com1_port_bytes_written));
3124  total_size += sizeof(diagnostic_field.com1_port_bytes_written);
3125  byteswap_inplace(&response_buffer[total_size],
3126  sizeof(diagnostic_field.com1_port_bytes_read));
3127  total_size += sizeof(diagnostic_field.com1_port_bytes_read);
3128  byteswap_inplace(&response_buffer[total_size],
3129  sizeof(diagnostic_field.com1_port_write_overruns));
3130  total_size += sizeof(diagnostic_field.com1_port_write_overruns);
3131  byteswap_inplace(&response_buffer[total_size],
3132  sizeof(diagnostic_field.com1_port_read_overruns));
3133  total_size += sizeof(diagnostic_field.com1_port_read_overruns);
3134  byteswap_inplace(&response_buffer[total_size],
3135  sizeof(diagnostic_field.imu_parser_errors));
3136  total_size += sizeof(diagnostic_field.imu_parser_errors);
3137  byteswap_inplace(&response_buffer[total_size],
3138  sizeof(diagnostic_field.imu_message_count));
3139  total_size += sizeof(diagnostic_field.imu_message_count);
3140  byteswap_inplace(&response_buffer[total_size],
3141  sizeof(diagnostic_field.imu_last_message_ms));
3142  }
3143 
3144  void * struct_pointer;
3145  struct_pointer = &diagnostic_field;
3146  total_size = 0;
3147 
3148  memcpy(struct_pointer, response_buffer,
3149  sizeof(diagnostic_field.device_model));
3150  total_size += sizeof(diagnostic_field.device_model);
3151  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3152  sizeof(diagnostic_field.status_selector));
3153  total_size += sizeof(diagnostic_field.status_selector);
3154  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3155  sizeof(diagnostic_field.status_flags));
3156  total_size += sizeof(diagnostic_field.status_flags);
3157  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3158  sizeof(diagnostic_field.system_state));
3159  total_size += sizeof(diagnostic_field.system_state);
3160  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3161  sizeof(diagnostic_field.system_timer_ms));
3162  total_size += sizeof(diagnostic_field.system_timer_ms);
3163  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3164  sizeof(diagnostic_field.imu_stream_enabled));
3165  total_size += sizeof(diagnostic_field.imu_stream_enabled);
3166  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3167  sizeof(diagnostic_field.filter_stream_enabled));
3168  total_size += sizeof(diagnostic_field.filter_stream_enabled);
3169  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3170  sizeof(diagnostic_field.imu_dropped_packets));
3171  total_size += sizeof(diagnostic_field.imu_dropped_packets);
3172  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3173  sizeof(diagnostic_field.filter_dropped_packets));
3174  total_size += sizeof(diagnostic_field.filter_dropped_packets);
3175  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3176  sizeof(diagnostic_field.com1_port_bytes_written));
3177  total_size += sizeof(diagnostic_field.com1_port_bytes_written);
3178  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3179  sizeof(diagnostic_field.com1_port_bytes_read));
3180  total_size += sizeof(diagnostic_field.com1_port_bytes_read);
3181  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3182  sizeof(diagnostic_field.com1_port_write_overruns));
3183  total_size += sizeof(diagnostic_field.com1_port_write_overruns);
3184  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3185  sizeof(diagnostic_field.com1_port_read_overruns));
3186  total_size += sizeof(diagnostic_field.com1_port_read_overruns);
3187  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3188  sizeof(diagnostic_field.imu_parser_errors));
3189  total_size += sizeof(diagnostic_field.imu_parser_errors);
3190  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3191  sizeof(diagnostic_field.imu_message_count));
3192  total_size += sizeof(diagnostic_field.imu_message_count);
3193  memcpy((struct_pointer + total_size), &(response_buffer[total_size]),
3194  sizeof(diagnostic_field.imu_last_message_ms));
3195  total_size += sizeof(diagnostic_field.imu_last_message_ms);
3196  }
3197  else
3198  return MIP_INTERFACE_ERROR;
3199 
3200  return MIP_INTERFACE_OK;
3201  }
3202  }
3203 
3204  // Start callbacks for data packets
3205 
3207  //
3208  // Filter Packet Callback
3209  //
3211  void Microstrain::filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3212  {
3213  mip_field_header *field_header;
3214  u8 *field_data;
3215  u16 field_offset = 0;
3216 
3217  // If we aren't publishing, then return
3218  if (!publish_odom_ && !publish_filtered_imu_)
3219  return;
3220 
3221  // ROS_INFO("Filter callback");
3222  // The packet callback can have several types, process them all
3223  switch (callback_type)
3224  {
3226  // Handle valid packets
3228  case MIP_INTERFACE_CALLBACK_VALID_PACKET:
3229  {
3230  filter_valid_packet_count_++;
3231 
3233  // Loop through all of the data fields
3235 
3236  while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
3237  {
3239  // Decode the field
3241 
3242  switch (field_header->descriptor)
3243  {
3245  // Estimated LLH Position
3247 
3248  case MIP_FILTER_DATA_LLH_POS:
3249  {
3250  memcpy(&curr_filter_pos_, field_data, sizeof(mip_filter_llh_pos));
3251 
3252  // For little-endian targets, byteswap the data field
3253  mip_filter_llh_pos_byteswap(&curr_filter_pos_);
3254 
3255  nav_msg_.header.seq = filter_valid_packet_count_;
3256  nav_msg_.header.stamp = ros::Time::now();
3257  nav_msg_.header.frame_id = odom_frame_id_;
3258  nav_msg_.child_frame_id = odom_child_frame_id_;
3259  nav_msg_.pose.pose.position.y = curr_filter_pos_.latitude;
3260  nav_msg_.pose.pose.position.x = curr_filter_pos_.longitude;
3261  nav_msg_.pose.pose.position.z = curr_filter_pos_.ellipsoid_height;
3262  }
3263  break;
3264 
3266  // Estimated NED Velocity
3268 
3269  case MIP_FILTER_DATA_NED_VEL:
3270  {
3271  memcpy(&curr_filter_vel_, field_data, sizeof(mip_filter_ned_velocity));
3272 
3273  // For little-endian targets, byteswap the data field
3274  mip_filter_ned_velocity_byteswap(&curr_filter_vel_);
3275 
3276  // rotate velocities from NED to sensor coordinates
3277  // Constructor takes x, y, z , w
3278  tf2::Quaternion nav_quat(curr_filter_quaternion_.q[2],
3279  curr_filter_quaternion_.q[1],
3280  -1.0*curr_filter_quaternion_.q[3],
3281  curr_filter_quaternion_.q[0]);
3282 
3283  tf2::Vector3 vel_enu(curr_filter_vel_.east,
3284  curr_filter_vel_.north,
3285  -1.0*curr_filter_vel_.down);
3286  tf2::Vector3 vel_in_sensor_frame = tf2::quatRotate(nav_quat.inverse(), vel_enu);
3287 
3288  nav_msg_.twist.twist.linear.x = vel_in_sensor_frame[0]; // curr_filter_vel_.east;
3289  nav_msg_.twist.twist.linear.y = vel_in_sensor_frame[1]; // curr_filter_vel_.north;
3290  nav_msg_.twist.twist.linear.z = vel_in_sensor_frame[2]; // -1*curr_filter_vel_.down;
3291  }
3292  break;
3293 
3295  // Estimated Attitude, Euler Angles
3297 
3298  case MIP_FILTER_DATA_ATT_EULER_ANGLES:
3299  {
3300  memcpy(&curr_filter_angles_, field_data, sizeof(mip_filter_attitude_euler_angles));
3301 
3302  // For little-endian targets, byteswap the data field
3303  mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles_);
3304  }
3305  break;
3306 
3307  // Quaternion
3308  case MIP_FILTER_DATA_ATT_QUATERNION:
3309  {
3310  memcpy(&curr_filter_quaternion_, field_data, sizeof(mip_filter_attitude_quaternion));
3311 
3312  // For little-endian targets, byteswap the data field
3313  mip_filter_attitude_quaternion_byteswap(&curr_filter_quaternion_);
3314 
3315  // If we want the orientation to be based on the reference on the imu
3316  tf2::Quaternion q(curr_filter_quaternion_.q[1],curr_filter_quaternion_.q[2],
3317  curr_filter_quaternion_.q[3],curr_filter_quaternion_.q[0]);
3318  geometry_msgs::Quaternion quat_msg;
3319 
3320  if(frame_based_enu_)
3321  {
3322  // Create a rotation from NED -> ENU
3323  tf2::Quaternion q_rotate;
3324  q_rotate.setRPY(M_PI,0.0,M_PI/2);
3325  // Apply the NED to ENU rotation such that the coordinate frame matches
3326  q = q_rotate*q;
3327  quat_msg = tf2::toMsg(q);
3328  }
3329  else
3330  {
3331  // put into ENU - swap X/Y, invert Z
3332  quat_msg.x = q[1];
3333  quat_msg.y = q[0];
3334  quat_msg.z = -1.0*q[2];
3335  quat_msg.w = q[3];
3336  }
3337 
3338  nav_msg_.pose.pose.orientation = quat_msg;
3339 
3340  if (publish_filtered_imu_)
3341  {
3342  // Header
3343  filtered_imu_msg_.header.seq = filter_valid_packet_count_;
3344  filtered_imu_msg_.header.stamp = ros::Time::now();
3345  filtered_imu_msg_.header.frame_id = imu_frame_id_;
3346  filtered_imu_msg_.orientation = nav_msg_.pose.pose.orientation;
3347  }
3348  }
3349  break;
3350 
3351  // Angular Rates
3352  case MIP_FILTER_DATA_COMPENSATED_ANGULAR_RATE:
3353  {
3354  memcpy(&curr_filter_angular_rate_, field_data, sizeof(mip_filter_compensated_angular_rate));
3355 
3356  // For little-endian targets, byteswap the data field
3357  mip_filter_compensated_angular_rate_byteswap(&curr_filter_angular_rate_);
3358 
3359  nav_msg_.twist.twist.angular.x = curr_filter_angular_rate_.x;
3360  nav_msg_.twist.twist.angular.y = curr_filter_angular_rate_.y;
3361  nav_msg_.twist.twist.angular.z = curr_filter_angular_rate_.z;
3362 
3363  if (publish_filtered_imu_)
3364  {
3365  filtered_imu_msg_.angular_velocity.x = curr_filter_angular_rate_.x;
3366  filtered_imu_msg_.angular_velocity.y = curr_filter_angular_rate_.y;
3367  filtered_imu_msg_.angular_velocity.z = curr_filter_angular_rate_.z;
3368  }
3369  }
3370  break;
3371 
3372  // Position Uncertainty
3373  case MIP_FILTER_DATA_POS_UNCERTAINTY:
3374  {
3375  memcpy(&curr_filter_pos_uncertainty_, field_data, sizeof(mip_filter_llh_pos_uncertainty));
3376 
3377  // For little-endian targets, byteswap the data field
3378  mip_filter_llh_pos_uncertainty_byteswap(&curr_filter_pos_uncertainty_);
3379 
3380  // x-axis
3381  nav_msg_.pose.covariance[0] = curr_filter_pos_uncertainty_.east*curr_filter_pos_uncertainty_.east;
3382  nav_msg_.pose.covariance[7] = curr_filter_pos_uncertainty_.north*curr_filter_pos_uncertainty_.north;
3383  nav_msg_.pose.covariance[14] = curr_filter_pos_uncertainty_.down*curr_filter_pos_uncertainty_.down;
3384  }
3385  break;
3386 
3387  // Velocity Uncertainty
3388  case MIP_FILTER_DATA_VEL_UNCERTAINTY:
3389  {
3390  memcpy(&curr_filter_vel_uncertainty_, field_data, sizeof(mip_filter_ned_vel_uncertainty));
3391 
3392  // For little-endian targets, byteswap the data field
3393  mip_filter_ned_vel_uncertainty_byteswap(&curr_filter_vel_uncertainty_);
3394 
3395  nav_msg_.twist.covariance[0] = curr_filter_vel_uncertainty_.east*curr_filter_vel_uncertainty_.east;
3396  nav_msg_.twist.covariance[7] = curr_filter_vel_uncertainty_.north*curr_filter_vel_uncertainty_.north;
3397  nav_msg_.twist.covariance[14] = curr_filter_vel_uncertainty_.down*curr_filter_vel_uncertainty_.down;
3398  }
3399  break;
3400 
3401  // Attitude Uncertainty
3402  case MIP_FILTER_DATA_ATT_UNCERTAINTY_EULER:
3403  {
3404  memcpy(&curr_filter_att_uncertainty_, field_data, sizeof(mip_filter_euler_attitude_uncertainty));
3405 
3406  // For little-endian targets, byteswap the data field
3407  mip_filter_euler_attitude_uncertainty_byteswap(&curr_filter_att_uncertainty_);
3408  nav_msg_.pose.covariance[21] = curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll;
3409  nav_msg_.pose.covariance[28] = curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch;
3410  nav_msg_.pose.covariance[35] = curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw;
3411 
3412  if (publish_filtered_imu_)
3413  {
3414  filtered_imu_msg_.orientation_covariance[0] =
3415  curr_filter_att_uncertainty_.roll*curr_filter_att_uncertainty_.roll;
3416  filtered_imu_msg_.orientation_covariance[4] =
3417  curr_filter_att_uncertainty_.pitch*curr_filter_att_uncertainty_.pitch;
3418  filtered_imu_msg_.orientation_covariance[8] =
3419  curr_filter_att_uncertainty_.yaw*curr_filter_att_uncertainty_.yaw;
3420  }
3421  }
3422  break;
3423 
3424  // Filter Status
3425  case MIP_FILTER_DATA_FILTER_STATUS:
3426  {
3427  memcpy(&curr_filter_status_, field_data, sizeof(mip_filter_status));
3428 
3429  // For little-endian targets, byteswap the data field
3430  mip_filter_status_byteswap(&curr_filter_status_);
3431 
3432  nav_status_msg_.data.clear();
3433  ROS_DEBUG_THROTTLE(1.0, "Filter Status: %#06X, Dyn. Mode: %#06X, Filter State: %#06X",
3434  curr_filter_status_.filter_state,
3435  curr_filter_status_.dynamics_mode,
3436  curr_filter_status_.status_flags);
3437  nav_status_msg_.data.push_back(curr_filter_status_.filter_state);
3438  nav_status_msg_.data.push_back(curr_filter_status_.dynamics_mode);
3439  nav_status_msg_.data.push_back(curr_filter_status_.status_flags);
3440  nav_status_pub_.publish(nav_status_msg_);
3441  }
3442  break;
3443 
3445  // Scaled Accelerometer
3447 
3448  case MIP_FILTER_DATA_LINEAR_ACCELERATION:
3449  {
3450  memcpy(&curr_filter_linear_accel_, field_data, sizeof(mip_filter_linear_acceleration));
3451 
3452  // For little-endian targets, byteswap the data field
3453  mip_filter_linear_acceleration_byteswap(&curr_filter_linear_accel_);
3454 
3455  // If we want gravity removed, use this as acceleration
3456  if (remove_imu_gravity_)
3457  {
3458  // Stuff into ROS message - acceleration already in m/s^2
3459  filtered_imu_msg_.linear_acceleration.x = curr_filter_linear_accel_.x;
3460  filtered_imu_msg_.linear_acceleration.y = curr_filter_linear_accel_.y;
3461  filtered_imu_msg_.linear_acceleration.z = curr_filter_linear_accel_.z;
3462  }
3463  // Otherwise, do nothing with this packet
3464  }
3465  break;
3466 
3467  case MIP_FILTER_DATA_COMPENSATED_ACCELERATION:
3468  {
3469  memcpy(&curr_filter_accel_comp_, field_data, sizeof(mip_filter_compensated_acceleration));
3470 
3471  // For little-endian targets, byteswap the data field
3472  mip_filter_compensated_acceleration_byteswap(&curr_filter_accel_comp_);
3473 
3474  // If we do not want to have gravity removed, use this as acceleration
3475  if (!remove_imu_gravity_)
3476  {
3477  // Stuff into ROS message - acceleration already in m/s^2
3478  filtered_imu_msg_.linear_acceleration.x = curr_filter_accel_comp_.x;
3479  filtered_imu_msg_.linear_acceleration.y = curr_filter_accel_comp_.y;
3480  filtered_imu_msg_.linear_acceleration.z = curr_filter_accel_comp_.z;
3481  }
3482  // Otherwise, do nothing with this packet
3483  }
3484  break;
3485 
3486  default: break;
3487  }
3488  }
3489 
3490  // Publish
3491  if (publish_odom_)
3492  {
3493  nav_pub_.publish(nav_msg_);
3494  }
3495 
3496  if (publish_filtered_imu_)
3497  {
3498  // Does it make sense to get the angular velocity bias and acceleration bias to populate these?
3499  // Since the sensor does not produce a covariance for linear acceleration, set it based
3500  // on our pulled in parameters.
3501  std::copy(imu_linear_cov_.begin(), imu_linear_cov_.end(),
3502  filtered_imu_msg_.linear_acceleration_covariance.begin());
3503  // Since the sensor does not produce a covariance for angular velocity, set it based
3504  // on our pulled in parameters.
3505  std::copy(imu_angular_cov_.begin(), imu_angular_cov_.end(),
3506  filtered_imu_msg_.angular_velocity_covariance.begin());
3507  filtered_imu_pub_.publish(filtered_imu_msg_);
3508  }
3509  }
3510  break;
3511 
3513  // Handle checksum error packets
3515 
3516  case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
3517  {
3518  filter_checksum_error_packet_count_++;
3519  }
3520  break;
3521 
3523  // Handle timeout packets
3525 
3526  case MIP_INTERFACE_CALLBACK_TIMEOUT:
3527  {
3528  filter_timeout_packet_count_++;
3529  }
3530  break;
3531 
3532  default: break;
3533  }
3534 
3535  print_packet_stats();
3536  } // filter_packet_callback
3537 
3538 
3539 
3540  // Send diagnostic information to device status topic and diagnostic aggregator
3541  void Microstrain::device_status_callback()
3542  {
3543  if (GX5_25)
3544  {
3545  u8 response_buffer[sizeof(gx4_25_diagnostic_device_status_field)];
3546  start = clock();
3547  while (mip_3dm_cmd_hw_specific_device_status(&device_interface_,
3548  GX4_25_MODEL_NUMBER, GX4_25_DIAGNOSTICS_STATUS_SEL,
3549  response_buffer) != MIP_INTERFACE_OK)
3550  {
3551  if (clock() - start > 5000)
3552  {
3553  ROS_INFO("mip_3dm_cmd_hw_specific_device_status function timed out.");
3554  break;
3555  }
3556  }
3557 
3558  // ROS_INFO("Adding device diagnostics to status msg");
3559  // ROS_INFO("adding data to message");
3560  device_status_msg_.device_model = diagnostic_field.device_model;
3561  device_status_msg_.status_selector = diagnostic_field.status_selector;
3562  device_status_msg_.status_flags = diagnostic_field.status_flags;
3563  device_status_msg_.system_state = diagnostic_field.system_state;
3564  device_status_msg_.system_timer_ms = diagnostic_field.system_timer_ms;
3565  device_status_msg_.imu_stream_enabled = diagnostic_field.imu_stream_enabled;
3566  device_status_msg_.filter_stream_enabled = diagnostic_field.filter_stream_enabled;
3567  device_status_msg_.imu_dropped_packets = diagnostic_field.imu_dropped_packets;
3568  device_status_msg_.filter_dropped_packets = diagnostic_field.filter_dropped_packets;
3569  device_status_msg_.com1_port_bytes_written = diagnostic_field.com1_port_bytes_written;
3570  device_status_msg_.com1_port_bytes_read = diagnostic_field.com1_port_bytes_read;
3571  device_status_msg_.com1_port_write_overruns = diagnostic_field.com1_port_write_overruns;
3572  device_status_msg_.com1_port_read_overruns = diagnostic_field.com1_port_read_overruns;
3573  device_status_msg_.imu_parser_errors = diagnostic_field.imu_parser_errors;
3574  device_status_msg_.imu_message_count = diagnostic_field.imu_message_count;
3575  device_status_msg_.imu_last_message_ms = diagnostic_field.imu_last_message_ms;
3576 
3577  device_status_pub_.publish(device_status_msg_);
3578  }
3579  else
3580  {
3581  ROS_INFO("Device status messages not configured for this model");
3582  }
3583  }
3584 
3586  //
3587  // AHRS Packet Callback
3588  //
3590 
3591  void Microstrain::ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3592  {
3593  mip_field_header *field_header;
3594  u8 *field_data;
3595  u16 field_offset = 0;
3596  // If we aren't publishing, then return
3597  if (!publish_imu_)
3598  return;
3599  // The packet callback can have several types, process them all
3600  switch (callback_type)
3601  {
3603  // Handle valid packets
3605 
3606  case MIP_INTERFACE_CALLBACK_VALID_PACKET:
3607  {
3608  ahrs_valid_packet_count_++;
3609 
3611  // Loop through all of the data fields
3613 
3614  while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
3615  {
3617  // Decode the field
3619 
3620  switch (field_header->descriptor)
3621  {
3623  // Scaled Accelerometer
3625 
3626  case MIP_AHRS_DATA_ACCEL_SCALED:
3627  {
3628  memcpy(&curr_ahrs_accel_, field_data, sizeof(mip_ahrs_scaled_accel));
3629 
3630  // For little-endian targets, byteswap the data field
3631  mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel_);
3632 
3633  // Stuff into ROS message - acceleration in m/s^2
3634  // Header
3635  imu_msg_.header.seq = ahrs_valid_packet_count_;
3636  imu_msg_.header.stamp = ros::Time::now();
3637  imu_msg_.header.frame_id = imu_frame_id_;
3638  imu_msg_.linear_acceleration.x = 9.81*curr_ahrs_accel_.scaled_accel[0];
3639  imu_msg_.linear_acceleration.y = 9.81*curr_ahrs_accel_.scaled_accel[1];
3640  imu_msg_.linear_acceleration.z = 9.81*curr_ahrs_accel_.scaled_accel[2];
3641  // Since the sensor does not produce a covariance for linear acceleration,
3642  // set it based on our pulled in parameters.
3643  std::copy(imu_linear_cov_.begin(), imu_linear_cov_.end(),
3644  imu_msg_.linear_acceleration_covariance.begin());
3645  }
3646  break;
3647 
3649  // Scaled Gyro
3651 
3652  case MIP_AHRS_DATA_GYRO_SCALED:
3653  {
3654  memcpy(&curr_ahrs_gyro_, field_data, sizeof(mip_ahrs_scaled_gyro));
3655 
3656  // For little-endian targets, byteswap the data field
3657  mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro_);
3658 
3659  imu_msg_.angular_velocity.x = curr_ahrs_gyro_.scaled_gyro[0];
3660  imu_msg_.angular_velocity.y = curr_ahrs_gyro_.scaled_gyro[1];
3661  imu_msg_.angular_velocity.z = curr_ahrs_gyro_.scaled_gyro[2];
3662  // Since the sensor does not produce a covariance for angular velocity, set it based
3663  // on our pulled in parameters.
3664  std::copy(imu_angular_cov_.begin(), imu_angular_cov_.end(),
3665  imu_msg_.angular_velocity_covariance.begin());
3666  }
3667  break;
3668 
3670  // Scaled Magnetometer
3672 
3673  case MIP_AHRS_DATA_MAG_SCALED:
3674  {
3675  memcpy(&curr_ahrs_mag_, field_data, sizeof(mip_ahrs_scaled_mag));
3676 
3677  // For little-endian targets, byteswap the data field
3678  mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag_);
3679  }
3680  break;
3681 
3682  // Quaternion
3683  case MIP_AHRS_DATA_QUATERNION:
3684  {
3685  memcpy(&curr_ahrs_quaternion_, field_data, sizeof(mip_ahrs_quaternion));
3686 
3687  // For little-endian targets, byteswap the data field
3688  mip_ahrs_quaternion_byteswap(&curr_ahrs_quaternion_);
3689 
3690  // If we want the orientation to be based on the reference on the imu
3691  tf2::Quaternion q(curr_ahrs_quaternion_.q[1],curr_ahrs_quaternion_.q[2],
3692  curr_ahrs_quaternion_.q[3],curr_ahrs_quaternion_.q[0]);
3693  geometry_msgs::Quaternion quat_msg;
3694 
3695  if(frame_based_enu_)
3696  {
3697  // Create a rotation from NED -> ENU
3698  tf2::Quaternion q_rotate;
3699  q_rotate.setRPY(M_PI,0.0,M_PI/2);
3700  // Apply the NED to ENU rotation such that the coordinate frame matches
3701  q = q_rotate*q;
3702  quat_msg = tf2::toMsg(q);
3703  }
3704  else
3705  {
3706  // put into ENU - swap X/Y, invert Z
3707  quat_msg.x = q[1];
3708  quat_msg.y = q[0];
3709  quat_msg.z = -1.0*q[2];
3710  quat_msg.w = q[3];
3711  }
3712 
3713  imu_msg_.orientation = quat_msg;
3714 
3715 
3716 
3717  // Since the MIP_AHRS data does not contain uncertainty values
3718  // we have to set them based on the parameter values.
3719  std::copy(imu_orientation_cov_.begin(), imu_orientation_cov_.end(),
3720  imu_msg_.orientation_covariance.begin());
3721  }
3722  break;
3723 
3724  default: break;
3725  }
3726  }
3727 
3728  // Publish
3729  imu_pub_.publish(imu_msg_);
3730  }
3731  break;
3732 
3734  // Handle checksum error packets
3736 
3737  case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
3738  {
3739  ahrs_checksum_error_packet_count_++;
3740  }
3741  break;
3742 
3744  // Handle timeout packets
3746 
3747  case MIP_INTERFACE_CALLBACK_TIMEOUT:
3748  {
3749  ahrs_timeout_packet_count_++;
3750  }
3751  break;
3752 
3753  default: break;
3754  }
3755  print_packet_stats();
3756  } // ahrs_packet_callback
3757 
3758 
3760  //
3761  // GPS Packet Callback
3762  //
3764  void Microstrain::gps_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3765  {
3766  mip_field_header *field_header;
3767  u8 *field_data;
3768  u16 field_offset = 0;
3769  u8 msgvalid = 1; // keep track of message validity
3770 
3771  // If we aren't publishing, then return
3772  if (!publish_gps_)
3773  return;
3774  // The packet callback can have several types, process them all
3775  switch (callback_type)
3776  {
3778  // Handle valid packets
3780 
3781  case MIP_INTERFACE_CALLBACK_VALID_PACKET:
3782  {
3783  gps_valid_packet_count_++;
3784 
3786  // Loop through all of the data fields
3788 
3789  while (mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
3790  {
3792  // Decode the field
3794 
3795  switch (field_header->descriptor)
3796  {
3798  // LLH Position
3800 
3801  case MIP_GPS_DATA_LLH_POS:
3802  {
3803  memcpy(&curr_llh_pos_, field_data, sizeof(mip_gps_llh_pos));
3804 
3805  // For little-endian targets, byteswap the data field
3806  mip_gps_llh_pos_byteswap(&curr_llh_pos_);
3807 
3808  // push into ROS message
3809  gps_msg_.latitude = curr_llh_pos_.latitude;
3810  gps_msg_.longitude = curr_llh_pos_.longitude;
3811  gps_msg_.altitude = curr_llh_pos_.ellipsoid_height;
3812  gps_msg_.position_covariance_type = 2; // diagnals known
3813  gps_msg_.position_covariance[0] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy;
3814  gps_msg_.position_covariance[4] = curr_llh_pos_.horizontal_accuracy*curr_llh_pos_.horizontal_accuracy;
3815  gps_msg_.position_covariance[8] = curr_llh_pos_.vertical_accuracy*curr_llh_pos_.vertical_accuracy;
3816  gps_msg_.status.status = curr_llh_pos_.valid_flags - 1;
3817  gps_msg_.status.service = 1; // assumed
3818  // Header
3819  gps_msg_.header.seq = gps_valid_packet_count_;
3820  gps_msg_.header.stamp = ros::Time::now();
3821  gps_msg_.header.frame_id = gps_frame_id_;
3822  }
3823  break;
3824 
3826  // NED Velocity
3828 
3829  case MIP_GPS_DATA_NED_VELOCITY:
3830  {
3831  memcpy(&curr_ned_vel_, field_data, sizeof(mip_gps_ned_vel));
3832 
3833  // For little-endian targets, byteswap the data field
3834  mip_gps_ned_vel_byteswap(&curr_ned_vel_);
3835  }
3836  break;
3837 
3839  // GPS Time
3841 
3842  case MIP_GPS_DATA_GPS_TIME:
3843  {
3844  memcpy(&curr_gps_time_, field_data, sizeof(mip_gps_time));
3845 
3846  // For little-endian targets, byteswap the data field
3847  mip_gps_time_byteswap(&curr_gps_time_);
3848  }
3849  break;
3850 
3851  default: break;
3852  }
3853  }
3854  }
3855  break;
3856 
3858  // Handle checksum error packets
3860 
3861  case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
3862  {
3863  msgvalid = 0;
3864  gps_checksum_error_packet_count_++;
3865  }
3866  break;
3867 
3869  // Handle timeout packets
3871 
3872  case MIP_INTERFACE_CALLBACK_TIMEOUT:
3873  {
3874  msgvalid = 0;
3875  gps_timeout_packet_count_++;
3876  }
3877  break;
3878 
3879  default: break;
3880  }
3881 
3882  if (msgvalid)
3883  {
3884  // Publish the message
3885  gps_pub_.publish(gps_msg_);
3886  }
3887 
3888  print_packet_stats();
3889  } // gps_packet_callback
3890 
3891  void Microstrain::print_packet_stats()
3892  {
3893  ROS_DEBUG_THROTTLE(1.0, "%u FILTER (%u errors) %u AHRS (%u errors) %u GPS (%u errors) Packets",
3894  filter_valid_packet_count_, filter_timeout_packet_count_ + filter_checksum_error_packet_count_,
3895  ahrs_valid_packet_count_, ahrs_timeout_packet_count_ + ahrs_checksum_error_packet_count_,
3896  gps_valid_packet_count_, gps_timeout_packet_count_ + gps_checksum_error_packet_count_);
3897  } // print_packet_stats
3898 
3899 
3900 
3901 
3902  // Wrapper callbacks
3903  void filter_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3904  {
3905  Microstrain* ustrain = reinterpret_cast<Microstrain*>(user_ptr);
3906  ustrain->filter_packet_callback(user_ptr, packet, packet_size, callback_type);
3907  }
3908 
3909  void ahrs_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3910  {
3911  Microstrain* ustrain = reinterpret_cast<Microstrain*>(user_ptr);
3912  ustrain->ahrs_packet_callback(user_ptr, packet, packet_size, callback_type);
3913  }
3914  // Wrapper callbacks
3915  void gps_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
3916  {
3917  Microstrain* ustrain = reinterpret_cast<Microstrain*>(user_ptr);
3918  ustrain->gps_packet_callback(user_ptr, packet, packet_size, callback_type);
3919  }
3920 
3921 } // namespace Microstrain
Microstrain::filter_packet_callback_wrapper
void filter_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: microstrain_3dm.cpp:3903
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
ROS_FATAL
#define ROS_FATAL(...)
print_packet_stats
void print_packet_stats()
Definition: GX4-45_Test.c:2719
ros::NodeHandle
mip_sdk_port_close
u16 mip_sdk_port_close(void *port_handle)
Definition: mip_sdk_user_functions.c:349
ROS_DEBUG_THROTTLE
#define ROS_DEBUG_THROTTLE(rate,...)
gps_packet_callback
void gps_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: GX4-45_Test.c:2577
ros::Duration::sleep
bool sleep() const
tf2::Quaternion::inverse
Quaternion inverse() const
ahrs_packet_callback
void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: GX4-45_Test.c:2473
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ROS_WARN
#define ROS_WARN(...)
Microstrain::gps_packet_callback_wrapper
void gps_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: microstrain_3dm.cpp:3915
microstrain_diagnostic_updater.h
GX5_15_DEVICE
#define GX5_15_DEVICE
Definition: microstrain_3dm.h:82
Microstrain::ahrs_packet_callback_wrapper
void ahrs_packet_callback_wrapper(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: microstrain_3dm.cpp:3909
filter_packet_callback
void filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
Definition: GX4-45_Test.c:2368
ROS_INFO
#define ROS_INFO(...)
ros::Time::init
static void init()
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
microstrain_mips::RosDiagnosticUpdater
Definition: microstrain_diagnostic_updater.h:12
ros::ok
ROSCPP_DECL bool ok()
Sleep
#define Sleep(x)
Definition: GX4-25_Test.h:62
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MIP_SDK_GX4_45_IMU_STANDARD_MODE
#define MIP_SDK_GX4_45_IMU_STANDARD_MODE
Definition: GX4-45_Test.h:55
GX5_25_DEVICE
#define GX5_25_DEVICE
Definition: microstrain_3dm.h:81
ros::Rate::sleep
bool sleep()
tf2::toMsg
B toMsg(const A &a)
tf2::quatRotate
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
mip_3dm_cmd_hw_specific_device_status
u16 mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer)
Definition: GX4-45_Test.c:2843
ros::Time::now
static Time now()
device_interface
mip_interface device_interface
Definition: GX4-45_Test.c:60
GX5_45_DEVICE
#define GX5_45_DEVICE
Definition: microstrain_3dm.h:79
ros::spinOnce
ROSCPP_DECL void spinOnce()
ROS_ERROR
#define ROS_ERROR(...)
GX5_35_DEVICE
#define GX5_35_DEVICE
Definition: microstrain_3dm.h:80
DEFAULT_PACKET_TIMEOUT_MS
#define DEFAULT_PACKET_TIMEOUT_MS
Definition: GX4-25_Test.h:60

microstrain_mips
Author(s): Brian Bingham
autogenerated on Sun Dec 22 2019 03:54:45