GX4-45_Test.cpp
Go to the documentation of this file.
00001 
00002 //
00003 // GX4-45_Test.c
00004 //
00005 // Test program for the GX4-45
00006 //
00007 // Notes:  This program runs through most of the sdk functions supported
00008 //         by the GX4-45.  It does not permanently alter any of the device
00009 //         settings.
00010 //
00011 //
00012 // External dependencies:
00013 //
00014 //  mip_sdk.h
00015 //  GX4-45_Test.h
00016 //
00017 // Written By: Nathan Miller and Gregg Carpenter
00018 //
00020 //
00023 //
00033 //
00035 
00036 
00038 //
00039 //Include Files
00040 //
00042 
00043 #include "microstrain_3dm_gx5_45/GX4-45_Test.h"
00044 
00046 //
00047 // Defines
00048 //
00050 
00052 //
00053 // Globals
00054 //
00056 
00057 u8 enable_data_stats_output = 0;
00058 
00059 //The primary device interface structure
00060 mip_interface device_interface;
00061 
00062 //Packet Counters (valid, timeout, and checksum errors)
00063 u32 filter_valid_packet_count  = 0;
00064 u32 ahrs_valid_packet_count = 0;
00065 u32 gps_valid_packet_count  = 0;
00066 
00067 u32 filter_timeout_packet_count  = 0;
00068 u32 ahrs_timeout_packet_count = 0;
00069 u32 gps_timeout_packet_count  = 0;
00070 
00071 u32 filter_checksum_error_packet_count  = 0;
00072 u32 ahrs_checksum_error_packet_count = 0;
00073 u32 gps_checksum_error_packet_count  = 0;
00074 
00075 //Example data field storage
00076 
00077 //AHRS
00078 mip_ahrs_scaled_gyro  curr_ahrs_gyro;
00079 mip_ahrs_scaled_accel curr_ahrs_accel;
00080 mip_ahrs_scaled_mag   curr_ahrs_mag;
00081 
00082 //GPS
00083 mip_gps_llh_pos curr_llh_pos;
00084 mip_gps_ned_vel curr_ned_vel;
00085 mip_gps_time    curr_gps_time;
00086 
00087 //FILTER
00088 mip_filter_llh_pos               curr_filter_pos;
00089 mip_filter_ned_velocity          curr_filter_vel;
00090 mip_filter_attitude_euler_angles curr_filter_angles;
00091 
00092 
00094 //
00095 // Main Function
00096 //
00098 
00099 int main(int argc, char* argv[])
00100 {
00101  u32 com_port, baudrate;
00102  base_device_info_field device_info;
00103  u8  temp_string[20] = {0};
00104  u32 bit_result;
00105  u8  enable = 1;
00106  u8  data_stream_format_descriptors[10];
00107  u16 data_stream_format_decimation[10];
00108  u8  data_stream_format_num_entries = 0;
00109  u8  readback_data_stream_format_descriptors[10] = {0};
00110  u16 readback_data_stream_format_decimation[10]  = {0};
00111  u8  readback_data_stream_format_num_entries     =  0;
00112  u16 base_rate = 0;
00113  u16 device_descriptors[128]  = {0};
00114  u16 device_descriptors_size  = 128*2;
00115  s16 i;
00116  u16 j;
00117  u8  com_mode = 0;
00118  u8  readback_com_mode = 0;
00119  float angles[3]             = {0};
00120  float readback_angles[3]    = {0};
00121  float offset[3]             = {0};
00122  float readback_offset[3]    = {0};
00123  float hard_iron[3]          = {0};
00124  float hard_iron_readback[3] = {0};
00125  float soft_iron[9]          = {0};
00126  float soft_iron_readback[9] = {0};
00127  u8  dynamics_mode           = 0;
00128  u8  readback_dynamics_mode  = 0;
00129  u16 estimation_control   = 0, estimation_control_readback = 0;
00130  u8  gps_source     = 0;
00131  u8  heading_source = 0;
00132  u8  auto_init      = 0;
00133  float noise[3]          = {0};
00134  float readback_noise[3] = {0};
00135  float beta[3]                 = {0};
00136  float readback_beta[3]        = {0};
00137  mip_low_pass_filter_settings filter_settings;
00138  float bias_vector[3]              = {0};
00139  u16 duration = 0;
00140  gx4_imu_diagnostic_device_status_field imu_diagnostic_field;
00141  gx4_imu_basic_status_field imu_basic_field;
00142  gx4_45_diagnostic_device_status_field diagnostic_field;
00143  gx4_45_basic_status_field basic_field;
00144  mip_filter_external_gps_update_command external_gps_update;
00145  mip_filter_external_heading_update_command external_heading_update;
00146  mip_filter_zero_update_command zero_update_control, zero_update_readback;
00147  mip_filter_external_heading_with_time_command external_heading_with_time;
00148  mip_complementary_filter_settings comp_filter_command, comp_filter_readback;
00149 
00150  u8  declination_source_command, declination_source_readback;
00151 
00152  mip_filter_accel_magnitude_error_adaptive_measurement_command        accel_magnitude_error_command, accel_magnitude_error_readback;
00153  mip_filter_magnetometer_magnitude_error_adaptive_measurement_command mag_magnitude_error_command, mag_magnitude_error_readback;
00154  mip_filter_magnetometer_dip_angle_error_adaptive_measurement_command mag_dip_angle_error_command, mag_dip_angle_error_readback;
00155 
00156 
00157 
00159  //Verify the command line arguments
00161 
00162  if(argc != NUM_COMMAND_LINE_ARGUMENTS)
00163  {
00164   print_command_line_usage();
00165   return -1;
00166  }
00167 
00168  //Convert the arguments
00169  com_port = atoi(argv[1]);
00170  baudrate = atoi(argv[2]);
00171 
00173  //Initialize the interface to the device
00175  printf("Attempting to open interface on COM port %d \n",com_port);
00176  if(mip_interface_init(com_port, baudrate, &device_interface, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK)
00177   return -1;
00178 
00179 
00181  //jjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjj
00182  // IMU-Direct Mode Testing
00183  //
00185   
00186  com_mode = MIP_SDK_GX4_45_IMU_DIRECT_MODE;
00187 
00189  //Set communication mode
00191 
00192  printf("----------------------------------------------------------------------\n");
00193  printf("Attempting to set communications mode to IMU Direct mode\n");
00194  printf("----------------------------------------------------------------------\n\n");
00195  
00196  while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){}
00197 
00199  //Verify device mode setting
00201 
00202  while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK){}
00203 
00204  if(com_mode == MIP_SDK_GX4_45_IMU_DIRECT_MODE)
00205  {
00206 
00207   printf("Communications mode IMU Direct.\n");
00208 
00209   printf("\n\n");
00210   Sleep(1500);
00211 
00213   //
00214   //Base Command Tests
00215   //
00217   
00219   //Put the GX4-45 into idle mode
00221 
00222   printf("----------------------------------------------------------------------\n");
00223   printf("Idling Device\n");
00224   printf("----------------------------------------------------------------------\n\n");
00225  
00226   while(mip_base_cmd_idle(&device_interface) != MIP_INTERFACE_OK){}
00227 
00228   printf("\n\n");
00229   Sleep(1500);
00230 
00232   //Try to ping the GX4-45
00234 
00235   printf("----------------------------------------------------------------------\n");
00236   printf("Pinging Device\n");
00237   printf("----------------------------------------------------------------------\n\n");
00238  
00239   while(mip_base_cmd_ping(&device_interface) != MIP_INTERFACE_OK){}
00240 
00241   printf("\n\n");
00242   Sleep(1500);
00243 
00245   //Get the device information
00247 
00248   printf("----------------------------------------------------------------------\n");
00249   printf("Getting Device Information\n");
00250   printf("----------------------------------------------------------------------\n\n");
00251  
00252   while(mip_base_cmd_get_device_info(&device_interface, &device_info) != MIP_INTERFACE_OK){}
00253 
00254   printf("\n\nDevice Info:\n");
00255   printf("---------------------------------------------\n");
00256 
00257   memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00258   printf("Model Name       => %s\n", temp_string);
00259 
00260   memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00261   printf("Model Number     => %s\n", temp_string);
00262 
00263   memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00264   printf("Serial Number    => %s\n", temp_string);
00265 
00266   memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00267   printf("Lot Number       => %s\n", temp_string);
00268 
00269   memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00270   printf("Options          => %s\n", temp_string);
00271 
00272   printf("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000,
00273                                              (device_info.firmware_version)%1000/100,
00274                                              (device_info.firmware_version)%100);
00275  
00276   printf("\n\n");
00277   Sleep(1500);
00278  
00280   //Get the supported descriptors
00282 
00283   printf("----------------------------------------------------------------------\n");
00284   printf("Getting Supported descriptors\n");
00285   printf("----------------------------------------------------------------------\n\n");
00286  
00287   while(mip_base_cmd_get_device_supported_descriptors(&device_interface, (u8*)device_descriptors, &device_descriptors_size) != MIP_INTERFACE_OK){}
00288 
00289   printf("\n\nSupported descriptors:\n\n");
00290 
00291   for(i=0; i< device_descriptors_size/2; i++)
00292   {
00293    printf("Descriptor Set: %02x, Descriptor: %02x\n", device_descriptors[i] >> 8, device_descriptors[i]&0xFF);
00294    Sleep(100);
00295   }
00296 
00297   printf("\n\n");
00298   Sleep(1500);
00299  
00301   //Peform a built-in-test
00303 
00304   printf("----------------------------------------------------------------------\n");
00305   printf("Running Built In Test\n");
00306   printf("----------------------------------------------------------------------\n\n");
00307  
00308   while(mip_base_cmd_built_in_test(&device_interface, &bit_result) != MIP_INTERFACE_OK){}
00309 
00310   printf("\nBIT Result (should be 0x00000000) => 0x%08x\n\n", bit_result);
00311  
00312   printf("\n\n");
00313   Sleep(1500);
00314 
00316   //
00317   // 3DM Command Tests
00318   //
00320   
00322   //Get AHRS Base Rate
00324 
00325   printf("----------------------------------------------------------------------\n");
00326   printf("Getting the AHRS datastream base rate\n");
00327   printf("----------------------------------------------------------------------\n\n");
00328  
00329   while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00330 
00331   printf("\nAHRS Base Rate => %d Hz\n\n", base_rate);
00332  
00333   printf("\n\n");
00334   Sleep(1500);
00335  
00337   //Get Device Status
00339 
00340 
00341   printf("----------------------------------------------------------------------\n");
00342   printf("Requesting BASIC Status Report:\n");
00343   printf("----------------------------------------------------------------------\n\n");
00344  
00345   //Request basic status report
00346   while(mip_3dm_cmd_hw_specific_imu_device_status(&device_interface, GX4_IMU_MODEL_NUMBER, GX4_IMU_BASIC_STATUS_SEL, &imu_basic_field) != MIP_INTERFACE_OK){} 
00347 
00348   printf("Model Number: \t\t\t\t\t%04u\n", imu_basic_field.device_model);
00349   printf("Status Selector: \t\t\t\t%s\n", imu_basic_field.status_selector == GX4_IMU_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
00350   printf("Status Flags: \t\t\t\t\t0x%08x\n", imu_basic_field.status_flags);
00351   printf("System Millisecond Timer Count: \t\t%llu ms\n\n", imu_basic_field.system_timer_ms);
00352 
00353   printf("Requesting DIAGNOSTIC Status Report:\n");
00354 
00355   //Request diagnostic status report
00356   while(mip_3dm_cmd_hw_specific_imu_device_status(&device_interface, GX4_IMU_MODEL_NUMBER, GX4_IMU_DIAGNOSTICS_STATUS_SEL, &imu_diagnostic_field) != MIP_INTERFACE_OK){} 
00357 
00358   printf("Model Number: \t\t\t\t\t%04u\n", imu_diagnostic_field.device_model);
00359   printf("Status Selector: \t\t\t\t%s\n", imu_diagnostic_field.status_selector == GX4_IMU_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
00360   printf("Status Flags: \t\t\t\t\t0x%08x\n", imu_diagnostic_field.status_flags);
00361   printf("System Millisecond Timer Count: \t\t%llu ms\n", imu_diagnostic_field.system_timer_ms);
00362   printf("Magnetometer: \t\t\t\t\t%s\n", imu_diagnostic_field.has_mag == 1 ? "DETECTED" : "NOT-DETECTED");
00363   printf("Pressure Sensor: \t\t\t\t%s\n", imu_diagnostic_field.has_pressure == 1 ? "DETECTED" : "NOT-DETECTED");
00364   printf("Gyro Range Reported: \t\t\t\t%u deg/s\n", imu_diagnostic_field.gyro_range);
00365   printf("Accel Range Reported: \t\t\t\t%u G\n", imu_diagnostic_field.accel_range);
00366   printf("Magnetometer Range Reported: \t\t\t%f Gs\n", imu_diagnostic_field.mag_range);
00367   printf("Pressure Range Reported: \t\t\t%f hPa\n", imu_diagnostic_field.pressure_range);
00368   printf("Measured Internal Temperature: \t\t\t%f degrees C\n", imu_diagnostic_field.temp_degc);
00369   printf("Last Temperature Measured: \t\t\t%u ms\n", imu_diagnostic_field.last_temp_read_ms);
00370   printf("Bad Temperature Sensor Detected: \t\t%s\n", imu_diagnostic_field.temp_sensor_error == 1 ? "TRUE" : "FALSE");
00371   printf("Number Received GPS Pulse-Per-Second Pulses: \t%u Pulses\n", imu_diagnostic_field.num_gps_pps_triggers);
00372   printf("Time of Last GPS Pulse-Per-Second Pulse: \t%u ms\n", imu_diagnostic_field.last_gps_pps_trigger_ms);
00373   printf("Data Streaming Enabled: \t\t\t%s\n", imu_diagnostic_field.stream_enabled == 1 ? "TRUE" : "FALSE");
00374   printf("Number of Dropped Communication Packets: \t%u packets\n", imu_diagnostic_field.dropped_packets);
00375   printf("Communications Port Bytes Written: \t\t%u Bytes\n", imu_diagnostic_field.com_port_bytes_written);
00376   printf("Communications Port Bytes Read: \t\t%u Bytes\n", imu_diagnostic_field.com_port_bytes_read);
00377   printf("Communications Port Write Overruns: \t\t%u Bytes\n", imu_diagnostic_field.com_port_write_overruns);
00378   printf("Communications Port Read Overruns: \t\t%u Bytes\n", imu_diagnostic_field.com_port_read_overruns);
00379 
00380   printf("\n\n");
00381   Sleep(1500);
00382 
00384   //Set/Read Coning and Sculling Compensation
00386 
00387   printf("----------------------------------------------------------------------\n");
00388   printf("Disabling Coning and Sculling compensation\n");
00389   printf("----------------------------------------------------------------------\n\n");
00390  
00391   enable = MIP_3DM_CONING_AND_SCULLING_DISABLE;
00392 
00393   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00394 
00395   printf("Reading Coning and Sculling compensation enabled state:\n");
00396 
00397   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00398 
00399   printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00400 
00401   printf("Enabling Coning and Sculling compensation.\n");
00402 
00403   enable = MIP_3DM_CONING_AND_SCULLING_ENABLE;
00404 
00405   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00406 
00407   printf("Reading Coning and Sculling compensation enabled state:\n");
00408 
00409   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00410 
00411   printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00412 
00413   printf("\n\n");
00414   Sleep(1500);
00415  
00417   //Set/Read Accel Bias
00419 
00420   bias_vector[0] = 1.0f;
00421   bias_vector[1] = 2.0f;
00422   bias_vector[2] = 3.0f;
00423 
00424   printf("----------------------------------------------------------------------\n");
00425   printf("Accel Bias Vector\n");
00426   printf("----------------------------------------------------------------------\n\n");
00427   
00428   printf("Setting Accel Bias Vector:\n");
00429   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00430 
00431   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00432 
00433   memset(bias_vector, 0, 3*sizeof(float));
00434 
00435   printf("Reading current Accel Bias Vector:\n");
00436 
00437   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00438 
00439   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00440 
00441   printf("Resetting Accel Bias to default state.\n\n");
00442 
00443   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00444 
00445   printf("\n\n");
00446   Sleep(1500);
00447  
00449   //Set/Read Gyro Bias
00451 
00452   bias_vector[0] = 4.0f;
00453   bias_vector[1] = 5.0f;
00454   bias_vector[2] = 6.0f;
00455 
00456   printf("----------------------------------------------------------------------\n");
00457   printf("Gyro Bias Vector\n");
00458   printf("----------------------------------------------------------------------\n\n");
00459 
00460   printf("Setting Gyro Bias Vector:\n");
00461   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00462 
00463   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00464 
00465   memset(bias_vector, 0, 3*sizeof(float));
00466 
00467   printf("Reading current Gyro Bias Vector:\n");
00468 
00469   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00470 
00471   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00472 
00473   printf("Resetting Gyro Bias to default state.\n\n");
00474 
00475   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00476 
00477   printf("\n\n");
00478   Sleep(1500);
00479  
00480 
00482   //Capture Gyro Bias
00484 
00485   printf("----------------------------------------------------------------------\n");
00486   printf("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n");
00487   printf("----------------------------------------------------------------------\n\n");
00488  
00489   duration = 5000; //milliseconds
00490 
00491   while(mip_3dm_cmd_capture_gyro_bias(&device_interface, duration, bias_vector) != MIP_INTERFACE_OK){}
00492 
00493   printf("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00494 
00495   printf("\n\n");
00496   Sleep(1500);
00497 
00498 
00500   //Set/Read the hard iron offset values
00502 
00503   printf("----------------------------------------------------------------------\n");
00504   printf("Setting the hard iron offset values\n");
00505   printf("----------------------------------------------------------------------\n\n");
00506 
00507   hard_iron[0] = 1.0;
00508   hard_iron[1] = 2.0;
00509   hard_iron[2] = 3.0;
00510 
00511   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, hard_iron) != MIP_INTERFACE_OK){}
00512 
00513   //Read back the hard iron offset values
00514   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, hard_iron_readback) != MIP_INTERFACE_OK){}
00515 
00516   if((abs(hard_iron_readback[0] - hard_iron[0]) < 0.001) &&
00517      (abs(hard_iron_readback[1] - hard_iron[1]) < 0.001) &&
00518      (abs(hard_iron_readback[2] - hard_iron[2]) < 0.001))
00519   {
00520    printf("Hard iron offset values successfully set.\n");
00521   }
00522   else
00523   {
00524    printf("ERROR: Failed to set hard iron offset values!!!\n");
00525    printf("Sent values:     %f X %f Y %f Z\n", hard_iron[0], hard_iron[1], hard_iron[2]);
00526    printf("Returned values: %f X %f Y %f Z\n", hard_iron_readback[0], hard_iron_readback[1], hard_iron_readback[2]);
00527   }
00528 
00529   printf("\n\nLoading the default hard iron offset values.\n\n");
00530 
00531   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00532 
00533   printf("\n\n");
00534   Sleep(1500);
00535 
00536 
00538   //Set/Read the soft iron matrix values
00540 
00541   printf("----------------------------------------------------------------------\n");
00542   printf("Setting the soft iron matrix values\n");
00543   printf("----------------------------------------------------------------------\n\n");
00544 
00545   for(i=0; i<9; i++)
00546    soft_iron[i] = i;
00547  
00548   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK){}
00549 
00550   //Read back the soft iron matrix values
00551   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){}
00552 
00553   if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) &&
00554      (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) &&
00555      (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) &&
00556          (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) &&
00557      (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) &&
00558      (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) &&
00559          (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) &&
00560      (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) &&
00561      (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001))
00562   {
00563    printf("Soft iron matrix values successfully set.\n");
00564   }
00565   else
00566   {
00567    printf("ERROR: Failed to set hard iron values!!!\n");
00568    printf("Sent values:     [%f  %f  %f][%f  %f  %f][%f  %f  %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]);
00569    printf("Returned values: [%f  %f  %f][%f  %f  %f][%f  %f  %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], 
00570                                                                          soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
00571   }
00572 
00573   printf("\n\nLoading the default soft iron matrix values.\n\n");
00574 
00575   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00576 
00577   printf("\n\n");
00578   Sleep(1500);
00579 
00580 
00581 
00583   //Setup the AHRS datastream format
00585 
00586   printf("----------------------------------------------------------------------\n");
00587   printf("Setting the AHRS message format\n");
00588   printf("----------------------------------------------------------------------\n\n");
00589  
00590   data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED;
00591   data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED;
00592 
00593   data_stream_format_decimation[0]  = 0x64;
00594   data_stream_format_decimation[1]  = 0x64;
00595 
00596   data_stream_format_num_entries = 2;
00597 
00598   while(mip_3dm_cmd_ahrs_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries, data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
00599 
00600   printf("\n\n");
00601   Sleep(1500);
00602 
00603 
00605   //Poll the AHRS data
00607 
00608   printf("----------------------------------------------------------------------\n");
00609   printf("Polling AHRS Data.\n");
00610   printf("----------------------------------------------------------------------\n\n");
00611  
00612   while(mip_3dm_cmd_poll_ahrs(&device_interface, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK){}
00613 
00614  }
00615  else
00616  {
00617   printf("ERROR: IMU_Direct mode not established\n\n");
00618  }
00619 
00620  printf("\n\n");
00621  Sleep(1500);
00622  
00623 
00625  //
00626  // Standard Mode Tests
00627  //
00629   
00630  device_descriptors_size  = 128*2;
00631  com_mode = MIP_SDK_GX4_45_IMU_STANDARD_MODE;
00632 
00633  printf("----------------------------------------------------------------------\n");
00634  printf("Putting Device Into Standard Mode\n");
00635  printf("----------------------------------------------------------------------\n\n");
00636  
00637  //Set communication mode
00638  while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){}
00639 
00640  //Verify device mode setting
00641  while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK){}
00642 
00643  printf("\n\n");
00644  Sleep(1500);
00645  
00646 
00647  if(com_mode == MIP_SDK_GX4_45_IMU_STANDARD_MODE)
00648  {
00649 
00651   //
00652   //Base Command Tests
00653   //
00655 
00657   //Put the GX4-45 into idle mode
00659 
00660   printf("----------------------------------------------------------------------\n");
00661   printf("Idling Device\n");
00662   printf("----------------------------------------------------------------------\n\n");
00663  
00664   while(mip_base_cmd_idle(&device_interface) != MIP_INTERFACE_OK){}
00665 
00666   printf("\n\n");
00667   Sleep(1500);
00668  
00670   //Try to ping the GX4-45
00672 
00673   printf("----------------------------------------------------------------------\n");
00674   printf("Pinging Device\n");
00675   printf("----------------------------------------------------------------------\n\n");
00676  
00677   while(mip_base_cmd_ping(&device_interface) != MIP_INTERFACE_OK){}
00678 
00679   printf("\n\n");
00680   Sleep(1500);
00681  
00683   //Get the device information
00685 
00686   printf("----------------------------------------------------------------------\n");
00687   printf("Getting Device Information\n");
00688   printf("----------------------------------------------------------------------\n\n");
00689 
00690   while(mip_base_cmd_get_device_info(&device_interface, &device_info) != MIP_INTERFACE_OK){}
00691 
00692   printf("Device Info:\n");
00693   printf("---------------------------------------------\n");
00694 
00695   memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00696   printf("Model Name       => %s\n", temp_string);
00697 
00698   memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00699   printf("Model Number     => %s\n", temp_string);
00700 
00701   memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00702   printf("Serial Number    => %s\n", temp_string);
00703 
00704   memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00705   printf("Lot Number       => %s\n", temp_string);
00706 
00707   memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00708   printf("Options          => %s\n", temp_string);
00709 
00710   printf("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000,
00711                                              (device_info.firmware_version)%1000/100,
00712                                              (device_info.firmware_version)%100);
00713 
00714   printf("\n\n");
00715   Sleep(1500);
00716  
00718   //Get the supported descriptors
00720 
00721   printf("----------------------------------------------------------------------\n");
00722   printf("Getting Supported descriptors\n");
00723   printf("----------------------------------------------------------------------\n\n");
00724 
00725   while(mip_base_cmd_get_device_supported_descriptors(&device_interface, (u8*)device_descriptors, &device_descriptors_size) != MIP_INTERFACE_OK){}
00726 
00727   printf("\n\nSupported descriptors:\n\n");
00728 
00729   for(i=0; i< device_descriptors_size/2; i++)
00730   {
00731    printf("Descriptor Set: %02x, Descriptor: %02x\n", device_descriptors[i] >> 8, device_descriptors[i]&0xFF);
00732    Sleep(100);
00733   }
00734 
00735   printf("\n\n");
00736   Sleep(1500);
00737  
00738 
00740   //Peform a built-in-test
00742 
00743   printf("----------------------------------------------------------------------\n");
00744   printf("Running Built In Test\n");
00745   printf("----------------------------------------------------------------------\n\n");
00746 
00747   while(mip_base_cmd_built_in_test(&device_interface, &bit_result) != MIP_INTERFACE_OK){}
00748 
00749   printf("\nBIT Result (should be 0x00000000) => 0x%08x\n\n", bit_result);
00750  
00751   
00752   printf("\n\n");
00753   Sleep(1500);
00754 
00756   //
00757   // 3DM Command Tests
00758   //
00760   
00762   //Get AHRS Base Rate
00764 
00765   printf("----------------------------------------------------------------------\n");
00766   printf("Getting the AHRS datastream base rate\n");
00767   printf("----------------------------------------------------------------------\n\n");
00768 
00769   while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00770 
00771   printf("\nAHRS Base Rate => %d Hz\n", base_rate);
00772 
00773   printf("\n\n");
00774   Sleep(1500);
00775 
00777   //Get GPS Base Rate
00779 
00780   printf("----------------------------------------------------------------------\n");
00781   printf("Getting the GPS datastream base rate\n");
00782   printf("----------------------------------------------------------------------\n\n");
00783 
00784   while(mip_3dm_cmd_get_gps_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00785 
00786   printf("\nGPS Base Rate => %d Hz\n", base_rate);
00787  
00788   printf("\n\n");
00789   Sleep(1500);
00790  
00792   //Get Estimation Filter Base Rate
00794 
00795   printf("----------------------------------------------------------------------\n");
00796   printf("Getting the Estimation Filter datastream base rate\n");
00797   printf("----------------------------------------------------------------------\n\n");
00798 
00799   while(mip_3dm_cmd_get_filter_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00800 
00801   printf("\nFILTER Base Rate => %d Hz\n", base_rate);
00802  
00803   printf("\n\n");
00804   Sleep(1500);
00805  
00807   //Set/Read Coning and Sculling Compensation
00809 
00810   printf("----------------------------------------------------------------------\n");
00811   printf("Toggling Coning and Sculling compensation\n");
00812   printf("----------------------------------------------------------------------\n\n");
00813 
00814   enable = MIP_3DM_CONING_AND_SCULLING_DISABLE;
00815 
00816   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00817 
00818   printf("Reading Coning and Sculling compensation enabled state:\n");
00819 
00820   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00821 
00822   printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00823 
00824   printf("Enabling Coning and Sculling compensation.\n");
00825 
00826   enable = MIP_3DM_CONING_AND_SCULLING_ENABLE;
00827 
00828   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00829 
00830   printf("Reading Coning and Sculling compensation enabled state:\n");
00831 
00832   while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00833 
00834   printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00835 
00836   printf("\n\n");
00837   Sleep(1500);
00838  
00839 
00841   //Set/Read Accel Bias
00843 
00844   bias_vector[0] = 1.0f;
00845   bias_vector[1] = 2.0f;
00846   bias_vector[2] = 3.0f;
00847 
00848   printf("----------------------------------------------------------------------\n");
00849   printf("Accel Bias Vector\n");
00850   printf("----------------------------------------------------------------------\n\n");
00851 
00852   printf("Setting Accel Bias Vector:\n");
00853   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00854 
00855   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00856 
00857   memset(bias_vector, 0, 3*sizeof(float));
00858 
00859   printf("Reading current Accel Bias Vector:\n");
00860 
00861   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00862 
00863   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00864 
00865   printf("Resetting Accel Bias to default state.\n\n");
00866 
00867   while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00868 
00869   printf("\n\n");
00870   Sleep(1500);
00871  
00872 
00874   //Set/Read Gyro Bias
00876 
00877   bias_vector[0] = 4.0f;
00878   bias_vector[1] = 5.0f;
00879   bias_vector[2] = 6.0f;
00880 
00881   printf("----------------------------------------------------------------------\n");
00882   printf("Gyro Bias Vector\n");
00883   printf("----------------------------------------------------------------------\n\n");
00884 
00885   printf("Setting Gyro Bias Vector:\n");
00886   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00887 
00888   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00889 
00890   memset(bias_vector, 0, 3*sizeof(float));
00891 
00892   printf("Reading current Gyro Bias Vector:\n");
00893 
00894   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00895 
00896   printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00897 
00898   printf("Resetting Gyro Bias to default state.\n\n");
00899 
00900   while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00901 
00902   printf("\n\n");
00903   Sleep(1500);
00904  
00905 
00907   //Capture Gyro Bias
00909 
00910   printf("----------------------------------------------------------------------\n");
00911   printf("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n");
00912   printf("----------------------------------------------------------------------\n\n");
00913 
00914   duration = 5000; //milliseconds
00915 
00916   while(mip_3dm_cmd_capture_gyro_bias(&device_interface, duration, bias_vector) != MIP_INTERFACE_OK){}
00917 
00918   printf("Gyro Bias Captured:\nbias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00919 
00920   printf("\n\n");
00921   Sleep(1500);
00922  
00923 
00925   //Set/Read the hard iron offset values
00927 
00928   printf("----------------------------------------------------------------------\n");
00929   printf("Setting the hard iron offset values\n");
00930   printf("----------------------------------------------------------------------\n\n");
00931 
00932   hard_iron[0] = 1.0;
00933   hard_iron[1] = 2.0;
00934   hard_iron[2] = 3.0;
00935 
00936   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, hard_iron) != MIP_INTERFACE_OK){}
00937 
00938   //Read back the hard iron offset values
00939   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, hard_iron_readback) != MIP_INTERFACE_OK){}
00940 
00941   if((abs(hard_iron_readback[0] - hard_iron[0]) < 0.001) &&
00942      (abs(hard_iron_readback[1] - hard_iron[1]) < 0.001) &&
00943      (abs(hard_iron_readback[2] - hard_iron[2]) < 0.001))
00944   {
00945    printf("Hard iron offset values successfully set.\n");
00946   }
00947   else
00948   {
00949    printf("ERROR: Failed to set hard iron offset values!!!\n");
00950    printf("Sent values:     %f X %f Y %f Z\n", hard_iron[0], hard_iron[1], hard_iron[2]);
00951    printf("Returned values: %f X %f Y %f Z\n", hard_iron_readback[0], hard_iron_readback[1], hard_iron_readback[2]);
00952   }
00953 
00954   printf("\n\nLoading the default hard iron offset values.\n\n");
00955 
00956   while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00957 
00958   printf("\n\n");
00959   Sleep(1500);
00960 
00961 
00963   //Set/Read the soft iron matrix values
00965 
00966   printf("----------------------------------------------------------------------\n");
00967   printf("Setting the soft iron matrix values\n");
00968   printf("----------------------------------------------------------------------\n\n");
00969 
00970   for(i=0; i<9; i++)
00971    soft_iron[i] = i;
00972  
00973   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK){}
00974 
00975   //Read back the soft iron matrix values
00976   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){}
00977 
00978   if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) &&
00979      (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) &&
00980      (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) &&
00981          (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) &&
00982      (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) &&
00983      (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) &&
00984          (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) &&
00985      (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) &&
00986      (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001))
00987   {
00988    printf("Soft iron matrix values successfully set.\n");
00989   }
00990   else
00991   {
00992    printf("ERROR: Failed to set hard iron values!!!\n");
00993    printf("Sent values:     [%f  %f  %f][%f  %f  %f][%f  %f  %f]\n", soft_iron[0], soft_iron[1], soft_iron[2], soft_iron[3], soft_iron[4], soft_iron[5], soft_iron[6], soft_iron[7], soft_iron[8]);
00994    printf("Returned values: [%f  %f  %f][%f  %f  %f][%f  %f  %f]\n", soft_iron_readback[0], soft_iron_readback[1], soft_iron_readback[2], soft_iron_readback[3], soft_iron_readback[4], 
00995                                                                          soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
00996   }
00997 
00998   printf("\n\nLoading the default soft iron matrix values.\n\n");
00999 
01000   while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01001 
01002   printf("\n\n");
01003   Sleep(1500);
01004 
01005 
01006 
01008   //Set/Read the complementary filter values
01010 
01011   printf("----------------------------------------------------------------------\n");
01012   printf("Setting the complementary filter values\n");
01013   printf("----------------------------------------------------------------------\n\n");
01014 
01015   
01016   comp_filter_command.north_compensation_enable = 0;
01017   comp_filter_command.up_compensation_enable    = 0;
01018 
01019   comp_filter_command.north_compensation_time_constant = 30;
01020   comp_filter_command.up_compensation_time_constant    = 30;
01021  
01022   while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK){}
01023 
01024   //Read back the complementary filter values
01025   while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK){}
01026 
01027   if((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) &&
01028      (comp_filter_command.up_compensation_enable    == comp_filter_readback.up_compensation_enable) &&
01029          (abs(comp_filter_command.north_compensation_time_constant - comp_filter_readback.north_compensation_time_constant) < 0.001) &&
01030      (abs(comp_filter_command.up_compensation_time_constant    - comp_filter_readback.up_compensation_time_constant) < 0.001))
01031   {
01032    printf("Complementary filter values successfully set.\n");
01033   }
01034   else
01035   {
01036    printf("ERROR: Failed to set complementary filter values!!!\n");
01037    printf("Sent values:     Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_command.up_compensation_enable, comp_filter_command.north_compensation_enable,
01038                                                                                                                  comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant);
01039    printf("Returned values: Up Enable: %d North Enable: %d Up Time Constant: %f North Time Constant: %f \n", comp_filter_readback.up_compensation_enable, comp_filter_readback.north_compensation_enable,
01040                                                                                                                  comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant);
01041   }
01042 
01043   printf("\n\nLoading the default complementary filter values.\n\n");
01044 
01045   while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01046 
01047   printf("\n\n");
01048   Sleep(1500);
01049 
01050 
01052   //Get Device Status
01054 
01055   printf("----------------------------------------------------------------------\n");
01056   printf("Requesting BASIC Status Report\n");
01057   printf("----------------------------------------------------------------------\n\n");
01058 
01059   //Request basic status report
01060   while(mip_3dm_cmd_hw_specific_device_status(&device_interface, GX4_45_MODEL_NUMBER, GX4_45_BASIC_STATUS_SEL, &basic_field) != MIP_INTERFACE_OK){} 
01061 
01062   printf("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model);
01063   printf("Status Selector: \t\t\t\t%s\n", basic_field.status_selector == GX4_45_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
01064   printf("Status Flags: \t\t\t\t\t0x%08x\n", basic_field.status_flags);
01065   
01066   if(basic_field.system_state == GX4_45_SYSTEM_STATE_INIT)
01067   {
01068    strcpy(temp_string,"System Initialization");
01069   }
01070   else if(basic_field.system_state == GX4_45_SYSTEM_STATE_SENSOR_STARTUP)
01071   {
01072    strcpy(temp_string,"Sensor Start-up");  
01073   }
01074   else if(basic_field.system_state == GX4_45_SYSTEM_STATE_RUNNING)
01075   {
01076    strcpy(temp_string,"System Running");
01077   }
01078 
01079   printf("System State: \t\t\t\t\t%s\n",temp_string);
01080   printf("System Microsecond Timer Count: \t\t%llu ms\n\n", basic_field.system_timer_ms);
01081 
01082   printf("Requesting DIAGNOSTIC Status Report:\n");
01083 
01084   //Request diagnostic status report
01085   while(mip_3dm_cmd_hw_specific_device_status(&device_interface, GX4_45_MODEL_NUMBER, GX4_45_DIAGNOSTICS_STATUS_SEL, &diagnostic_field) != MIP_INTERFACE_OK){} 
01086 
01087   printf("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model);
01088   printf("Status Selector: \t\t\t\t%s\n", diagnostic_field.status_selector == GX4_45_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
01089   printf("Status Flags: \t\t\t\t\t0x%08x\n", diagnostic_field.status_flags);
01090   printf("System Millisecond Timer Count: \t\t%llu ms\n", diagnostic_field.system_timer_ms);
01091   printf("Number Received GPS Pulse-Per-Second Pulses: \t%u Pulses\n", diagnostic_field.num_gps_pps_triggers);
01092   printf("Time of Last GPS Pulse-Per-Second Pulse: \t%u ms\n", diagnostic_field.last_gps_pps_trigger_ms);
01093   printf("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE");
01094   printf("GPS Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.gps_stream_enabled == 1 ? "TRUE" : "FALSE");
01095   printf("FILTER Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.filter_stream_enabled == 1 ? "TRUE" : "FALSE");
01096   printf("Number of Dropped IMU Packets: \t\t\t%u packets\n", diagnostic_field.imu_dropped_packets);
01097   printf("Number of Dropped GPS Packets: \t\t\t%u packets\n", diagnostic_field.gps_dropped_packets);
01098   printf("Number of Dropped FILTER Packets: \t\t\t%u packets\n", diagnostic_field.filter_dropped_packets);
01099   printf("Communications Port Bytes Written: \t\t%u Bytes\n", diagnostic_field.com1_port_bytes_written);
01100   printf("Communications Port Bytes Read: \t\t%u Bytes\n", diagnostic_field.com1_port_bytes_read);
01101   printf("Communications Port Write Overruns: \t\t%u Bytes\n", diagnostic_field.com1_port_write_overruns);
01102   printf("Communications Port Read Overruns: \t\t%u Bytes\n", diagnostic_field.com1_port_read_overruns);
01103   printf("IMU Parser Errors: \t\t\t\t%u Errors\n", diagnostic_field.imu_parser_errors);
01104   printf("IMU Message Count: \t\t\t\t%u Messages\n", diagnostic_field.imu_message_count);
01105   printf("IMU Last Message Received: \t\t\t%u ms\n", diagnostic_field.imu_last_message_ms);
01106   printf("GPS Parser Errors: \t\t\t\t%u Errors\n", diagnostic_field.gps_parser_errors);
01107   printf("GPS Message Count: \t\t\t\t%u Messages\n", diagnostic_field.gps_message_count);
01108   printf("GPS Last Message Received: \t\t\t%u ms\n", diagnostic_field.gps_last_message_ms);
01109 
01110   printf("\n\n");
01111   Sleep(1500);
01112  
01114   //
01115   // Filter Command Tests
01116   //
01118  
01119 
01121   //Reset the filter
01123 
01124   printf("----------------------------------------------------------------------\n");
01125   printf("Resetting the Filter\n");
01126   printf("----------------------------------------------------------------------\n\n");
01127 
01128   while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01129  
01130   printf("\n\n");
01131   Sleep(1500);
01132  
01134   //Initialize the filter with Euler Angles
01136 
01137   printf("----------------------------------------------------------------------\n");
01138   printf("Initializing the Filter with Euler angles\n");
01139   printf("----------------------------------------------------------------------\n\n");
01140 
01141   angles[0] = angles[1] = angles[2] = 0;
01142 
01143   while(mip_filter_set_init_attitude(&device_interface, angles) != MIP_INTERFACE_OK){}
01144  
01145   printf("\n\n");
01146   Sleep(1500);
01147  
01149   //Reset the filter
01151 
01152   printf("----------------------------------------------------------------------\n");
01153   printf("Resetting the Filter\n");
01154   printf("----------------------------------------------------------------------\n\n");
01155 
01156   while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01157  
01158   printf("\n\n");
01159   Sleep(1500);
01160  
01162   //Initialize the filter with a heading
01164 
01165   printf("----------------------------------------------------------------------\n");
01166   printf("Initializing the Filter with a heading angle\n");
01167   printf("----------------------------------------------------------------------\n\n");
01168 
01169   while(mip_filter_set_init_heading(&device_interface, angles[0]) != MIP_INTERFACE_OK){}
01170  
01171   printf("\n\n");
01172   Sleep(1500);
01173  
01175   //Reset the filter
01177 
01178   printf("----------------------------------------------------------------------\n");
01179   printf("Resetting the Filter\n");
01180   printf("----------------------------------------------------------------------\n\n");
01181 
01182   while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01183  
01184   printf("\n\n");
01185   Sleep(1500);
01186  
01188   //Set/Read the Vehicle dynamics mode
01190 
01191   printf("----------------------------------------------------------------------\n");
01192   printf("Cycle through available Vehicle Dynamics Modes\n");
01193   printf("----------------------------------------------------------------------\n\n");
01194 
01195   //Loop through valid modes and set/read current come mode
01196   for(i=3; i>=1; i--)
01197   {
01198    //Set the dynamics mode
01199    dynamics_mode = i;
01200 
01201    while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK){}
01202 
01203    //Reset the readback_dynamics_mode variable
01204    readback_dynamics_mode = 0;
01205 
01206    //Read back the com mode
01207    while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){}
01208 
01209    if(dynamics_mode == readback_dynamics_mode)
01210    {
01211     printf("Vehicle dynamics mode successfully set to %d\n", dynamics_mode);
01212    }
01213    else
01214    {
01215     printf("ERROR: Failed to set vehicle dynamics mode to %d!!!\n", dynamics_mode);
01216    }
01217   }
01218 
01219   printf("\nLoading the default vehicle dynamics mode\n\n");
01220 
01221   while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01222 
01223   printf("\n\n");
01224   Sleep(1500);
01225 
01226 
01228   //Set/Read the Sensor to Vehicle Frame transformation
01230 
01231   printf("----------------------------------------------------------------------\n");
01232   printf("Setting the sensor to vehicle frame transformation\n");
01233   printf("----------------------------------------------------------------------\n\n");
01234 
01235   angles[0] = 3.14/4.0;
01236   angles[1] = 3.14/8.0;
01237   angles[2] = 3.14/12.0;
01238 
01239   while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK){}
01240 
01241   //Read back the transformation
01242   while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK){}
01243 
01244   if((abs(readback_angles[0]-angles[0]) < 0.001) &&
01245      (abs(readback_angles[1]-angles[1]) < 0.001) &&
01246      (abs(readback_angles[2]-angles[2]) < 0.001))
01247   {
01248    printf("Transformation successfully set.\n");
01249   }
01250   else
01251   {
01252    printf("ERROR: Failed to set transformation!!!\n");
01253    printf("Sent angles:     %f roll %f pitch %f yaw\n", angles[0], angles[1], angles[2]);
01254    printf("Returned angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]);
01255   }
01256 
01257   printf("\n\nLoading the default sensor to vehicle transformation.\n\n");
01258 
01259   while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01260 
01261   printf("\n\n");
01262   Sleep(1500);
01263  
01265   //Set/Read the Sensor to Vehicle Frame offset
01267 
01268   printf("----------------------------------------------------------------------\n");
01269   printf("Setting the sensor to vehicle frame offset\n");
01270   printf("----------------------------------------------------------------------\n\n");
01271 
01272   offset[0] = 1.0;
01273   offset[1] = 2.0;
01274   offset[2] = 3.0;
01275 
01276   while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK){}
01277 
01278   //Read back the transformation
01279   while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){}
01280 
01281   if((abs(readback_offset[0]-offset[0]) < 0.001) &&
01282      (abs(readback_offset[1]-offset[1]) < 0.001) &&
01283      (abs(readback_offset[2]-offset[2]) < 0.001))
01284   {
01285    printf("Offset successfully set.\n");
01286   }
01287   else
01288   {
01289    printf("ERROR: Failed to set offset!!!\n");
01290    printf("Sent offset:     %f X %f Y %f Z\n", offset[0], offset[1], offset[2]);
01291    printf("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
01292   }
01293 
01294   printf("\n\nLoading the default sensor to vehicle offset.\n\n");
01295 
01296   while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01297  
01298   printf("\n\n");
01299   Sleep(1500);
01300  
01302   //Set/Read the GPS antenna offset
01304 
01305   printf("----------------------------------------------------------------------\n");
01306   printf("Setting the GPS antenna offset\n");
01307   printf("----------------------------------------------------------------------\n\n");
01308 
01309   offset[0] = 1.0;
01310   offset[1] = 2.0;
01311   offset[2] = 3.0;
01312 
01313   while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK){}
01314 
01315   //Read back the transformation
01316   while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){}
01317 
01318   if((abs(readback_offset[0]-offset[0]) < 0.001) &&
01319      (abs(readback_offset[1]-offset[1]) < 0.001) &&
01320      (abs(readback_offset[2]-offset[2]) < 0.001))
01321   {
01322    printf("Offset successfully set.\n");
01323   }
01324   else
01325   {
01326    printf("ERROR: Failed to set offset!!!\n");
01327    printf("Sent offset:     %f X %f Y %f Z\n", offset[0], offset[1], offset[2]);
01328    printf("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
01329   }
01330 
01331   printf("\n\nLoading the default antenna offset.\n\n");
01332 
01333   while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01334 
01335   printf("\n\n");
01336   Sleep(1500);
01337  
01339   //Set/Read Estimation control bits
01341   
01342   printf("----------------------------------------------------------------------\n");
01343   printf("Cycling through Estimation Control Flags\n"); 
01344   printf("----------------------------------------------------------------------\n\n");
01345 
01346   //negate successive bitfields
01347   for(j=0; j < 0x0020 ; j++)
01348   {
01349 
01350    estimation_control = (0xFFFF & ~j);
01351 
01352    //Set control bits
01353    while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK){}
01354  
01355    //Readback set control bits
01356    while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK){} 
01357 
01358    if(estimation_control != estimation_control_readback)
01359    {
01360     printf("ERROR:\n");
01361         
01362     if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GYRO_BIAS) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GYRO_BIAS) == 0)
01363      printf("Gyroscope Bias Estimation NOT DISABLED\n");
01364         
01365     if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_ACCEL_BIAS) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_ACCEL_BIAS) == 0)
01366      printf("Accelerometer Bias Estimation NOT DISABLED\n");
01367         
01368     if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GYRO_SCALE_FACTOR) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GYRO_SCALE_FACTOR) == 0)
01369      printf("Gyroscope Scale Factor Estimation NOT DISABLED\n");
01370         
01371     if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_ACCEL_SCALE_FACTOR) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_ACCEL_SCALE_FACTOR) == 0)
01372      printf("Accelerometer Scale Factor Estimation NOT DISABLED\n");
01373         
01374     if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GPS_ANTENNA_OFFSET) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GPS_ANTENNA_OFFSET) == 0)
01375      printf("GPS Antenna Offset Estimation NOT DISABLED\n");    
01376    }
01377   }
01378 
01379   printf("\n\nResetting Estimation Control Flags to default state.\n\n");
01380 
01381   while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, &estimation_control) != MIP_INTERFACE_OK){}
01382  
01383   printf("\n\n");
01384   Sleep(1500);
01385  
01387   //Set/Read the gps source
01389 
01390   printf("----------------------------------------------------------------------\n");
01391   printf("Cycle through available GPS sources\n");
01392   printf("----------------------------------------------------------------------\n\n");
01393 
01394   //Loop through valid modes and set/read current come mode
01395   for(i=2; i>=1; i--)
01396   {
01397    //Set the GPS source
01398    gps_source = i;
01399 
01400    while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01401 
01402    //Read back the GPS source
01403    while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &gps_source) != MIP_INTERFACE_OK){}
01404 
01405    if(gps_source == i)
01406    {
01407     printf("GPS source successfully set to %d\n", i);
01408    }
01409    else
01410    {
01411     printf("ERROR: Failed to set GPS source to %d!!!\n", i);
01412    }
01413   }
01414 
01415   printf("\n\nLoading the default gps source.\n\n");
01416 
01417   while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01418  
01419   printf("\n\n");
01420   Sleep(1500);
01421  
01423   //External GPS Update
01425 
01426   //Set the GPS source to external GPS
01427   printf("----------------------------------------------------------------------\n");
01428   printf("Performing External GPS Update with externally supplied GPS information\n");
01429   printf("----------------------------------------------------------------------\n\n");
01430 
01431   gps_source = 0x2;
01432 
01433   while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01434 
01435   //Set GPS External Update parameters
01436   external_gps_update.tow = 0;
01437   external_gps_update.week_number = 0;
01438   external_gps_update.pos[0] = 44.43753433; //Latitude of LORD Microstrain Sensing Systems
01439   external_gps_update.pos[1] = -73.10612488; //Longitude of LORD Microstrain Sensing Systems
01440   external_gps_update.pos[2] = 134.029999; //Height Above Ellipsoid of LORD Microstrain Sensing Systems
01441   external_gps_update.vel[0] = 0.0; //North Velocity
01442   external_gps_update.vel[1] = 0.0; //East Velocity
01443   external_gps_update.vel[2] = 0.0; //Down Velocity
01444   external_gps_update.pos_1sigma[0] = 0.1; //North Position Standard Deviation
01445   external_gps_update.pos_1sigma[1] = 0.1; //East Position Standard Deviation
01446   external_gps_update.pos_1sigma[2] = 0.1; //Down Position Standard Deviation
01447   external_gps_update.vel_1sigma[0] = 0.1; //North Velocity Standard Deviation
01448   external_gps_update.vel_1sigma[1] = 0.1; //East Velocity Standard Deviation
01449   external_gps_update.vel_1sigma[2] = 0.1; //Down Velocity Standard Deviation
01450 
01451   while(mip_filter_external_gps_update(&device_interface, &external_gps_update) != MIP_INTERFACE_OK){}
01452 
01453   //Reset GPS source to internal GPS
01454   gps_source = 0x01;
01455 
01456   while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01457  
01458   printf("\n\n");
01459   Sleep(1500);
01460  
01462   //External Heading Update
01464 
01465   printf("----------------------------------------------------------------------\n");
01466   printf("Performing External Heading Update with externally supplied heading information\n");
01467   printf("----------------------------------------------------------------------\n\n");
01468 
01469   //Set device to external heading update mode
01470   heading_source = 0x3;
01471 
01472   while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK){} 
01473 
01474   external_heading_update.heading_angle = 0.0; //0 Radians
01475   external_heading_update.heading_angle_1sigma = 0.01; //Radian Standard Deviation in heading estimate
01476   external_heading_update.type = 0x1; //True Heading
01477 
01478   while(mip_filter_external_heading_update(&device_interface, &external_heading_update) != MIP_INTERFACE_OK){}
01479 
01480   //Set device to default heading update mode
01481   
01482   while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, &heading_source) != MIP_INTERFACE_OK){} 
01483  
01484   printf("\n\n");
01485   Sleep(1500);
01486  
01488   //Cycle through the available heading sources
01490 
01491   printf("----------------------------------------------------------------------\n");
01492   printf("Cycle through available Heading sources\n");
01493   printf("----------------------------------------------------------------------\n\n");
01494 
01495   //Loop through valid heading sources and set/read
01496   for(i=3; i>=0; i--)
01497   {
01498    //Set the heading source
01499    heading_source = i;
01500 
01501    mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source);
01502 
01503    //Read back the heading source
01504    while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &heading_source) != MIP_INTERFACE_OK){}
01505 
01506    if(heading_source == i)
01507    {
01508     printf("Heading source successfully set to %d\n", i);
01509    }
01510    else
01511    {
01512     printf("ERROR: Failed to set heading source to %d!!!\n", i);
01513    }
01514   }
01515 
01516   printf("\n\nLoading the default heading source.\n\n");
01517 
01518   while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01519  
01520   printf("\n\n");
01521   Sleep(1500);
01522  
01524   //Cycle through the available auto-init values
01526 
01527   printf("----------------------------------------------------------------------\n");
01528   printf("Cycle through available auto-init values\n");
01529   printf("----------------------------------------------------------------------\n\n");
01530 
01531   //Loop through valid heading sources and set/read
01532   for(i=1; i>=0; i--)
01533   {
01534    //Set the auto-init value
01535    auto_init = i;
01536 
01537    while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &auto_init) != MIP_INTERFACE_OK){}
01538 
01539    //Read back the auto-init value
01540    while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_READ, &auto_init) != MIP_INTERFACE_OK){}
01541 
01542    if(auto_init == i)
01543    {
01544     printf("Auto-init successfully set to %d\n", i);
01545    }
01546    else
01547    {
01548     printf("ERROR: Failed to set auto-init to %d!!!\n", i);
01549    }
01550   }
01551 
01552   printf("\n\nLoading the default auto-init value.\n\n");
01553 
01554   while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01555  
01556   printf("\n\n");
01557   Sleep(1500);
01558  
01560   //Set/Read the accel noise values
01562 
01563   printf("----------------------------------------------------------------------\n");
01564   printf("Setting the accel noise values\n");
01565   printf("----------------------------------------------------------------------\n\n");
01566 
01567   noise[0] = 0.1;
01568   noise[1] = 0.2;
01569   noise[2] = 0.3;
01570 
01571   while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01572 
01573   //Read back the accel noise values
01574   while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01575 
01576   if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01577      (abs(readback_noise[1]-noise[1]) < 0.001) &&
01578      (abs(readback_noise[2]-noise[2]) < 0.001))
01579   {
01580    printf("Accel noise values successfully set.\n");
01581   }
01582   else
01583   {
01584    printf("ERROR: Failed to set accel noise values!!!\n");
01585    printf("Sent values:     %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01586    printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01587   }
01588 
01589   printf("\n\nLoading the default accel noise values.\n\n");
01590 
01591   while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01592  
01593   printf("\n\n");
01594   Sleep(1500);
01595  
01597   //Set/Read the gyro noise values
01599 
01600   printf("----------------------------------------------------------------------\n");
01601   printf("Setting the gyro noise values\n");
01602   printf("----------------------------------------------------------------------\n\n");
01603 
01604   noise[0] = 0.1;
01605   noise[1] = 0.2;
01606   noise[2] = 0.3;
01607 
01608   while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01609 
01610   //Read back the gyro noise values
01611   while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01612 
01613   if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01614      (abs(readback_noise[1]-noise[1]) < 0.001) &&
01615      (abs(readback_noise[2]-noise[2]) < 0.001))
01616   {
01617    printf("Gyro noise values successfully set.\n");
01618   }
01619   else
01620   {
01621    printf("ERROR: Failed to set gyro noise values!!!\n");
01622    printf("Sent values:     %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01623    printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01624   }
01625 
01626   printf("\n\nLoading the default gyro noise values.\n\n");
01627 
01628   while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01629  
01630   printf("\n\n");
01631   Sleep(1500);
01632  
01633 
01635   //Set/Read the mag noise values
01637 
01638   printf("----------------------------------------------------------------------\n");
01639   printf("Setting the mag noise values\n");
01640   printf("----------------------------------------------------------------------\n\n");
01641 
01642   noise[0] = 0.1;
01643   noise[1] = 0.2;
01644   noise[2] = 0.3;
01645 
01646   while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01647 
01648   //Read back the mag white noise values
01649   while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01650 
01651   if((abs(readback_noise[0] - noise[0]) < 0.001) &&
01652      (abs(readback_noise[1] - noise[1]) < 0.001) &&
01653      (abs(readback_noise[2] - noise[2]) < 0.001))
01654   {
01655    printf("Mag noise values successfully set.\n");
01656   }
01657   else
01658   {
01659    printf("ERROR: Failed to set mag noise values!!!\n");
01660    printf("Sent values:     %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01661    printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01662   }
01663 
01664   printf("\n\nLoading the default mag noise values.\n\n");
01665 
01666   while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01667 
01668   printf("\n\n");
01669   Sleep(1500);
01670 
01672   //Set/Read the accel bias model values
01674 
01675   printf("----------------------------------------------------------------------\n");
01676   printf("Setting the accel bias model values\n");
01677   printf("----------------------------------------------------------------------\n\n");
01678 
01679   noise[0] = 0.1;
01680   noise[1] = 0.2;
01681   noise[2] = 0.3;
01682 
01683   beta[0]        = 0.1;
01684   beta[1]        = 0.2;
01685   beta[2]        = 0.3;
01686 
01687   while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){}
01688 
01689   //Read back the accel bias model values
01690   while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){}
01691 
01692   if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01693      (abs(readback_noise[1]-noise[1]) < 0.001) &&
01694      (abs(readback_noise[2]-noise[2]) < 0.001) &&
01695      (abs(readback_beta[0]-beta[0]) < 0.001) &&
01696      (abs(readback_beta[1]-beta[1]) < 0.001) &&
01697      (abs(readback_beta[2]-beta[2]) < 0.001))
01698   {
01699    printf("Accel bias model values successfully set.\n");
01700   }
01701   else
01702   {
01703    printf("ERROR: Failed to set accel bias model values!!!\n");
01704    printf("Sent values:     Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]);
01705    printf("Returned values:  Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2],
01706                                                                                    readback_noise[0], readback_noise[1], readback_noise[2]);
01707   }
01708 
01709   printf("\n\nLoading the default accel bias model values.\n\n");
01710 
01711   while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL, NULL) != MIP_INTERFACE_OK){}
01712 
01713   printf("\n\n");
01714   Sleep(1500);
01715  
01717   //Set/Read the gyro bias model values
01719 
01720   printf("----------------------------------------------------------------------\n");
01721   printf("Setting the gyro bias model values\n");
01722   printf("----------------------------------------------------------------------\n\n");
01723 
01724   noise[0] = 0.1;
01725   noise[1] = 0.2;
01726   noise[2] = 0.3;
01727 
01728   beta[0] = 0.1;
01729   beta[1] = 0.2;
01730   beta[2] = 0.3;
01731 
01732   while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){}
01733 
01734   //Read back the gyro bias model values
01735   while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){}
01736 
01737   if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01738      (abs(readback_noise[1]-noise[1]) < 0.001) &&
01739      (abs(readback_noise[2]-noise[2]) < 0.001) &&
01740      (abs(readback_beta[0]-beta[0]) < 0.001) &&
01741      (abs(readback_beta[1]-beta[1]) < 0.001) &&
01742      (abs(readback_beta[2]-beta[2]) < 0.001))
01743   {
01744    printf("Gyro bias model values successfully set.\n");
01745   }
01746   else
01747   {
01748    printf("ERROR: Failed to set gyro bias model values!!!\n");
01749    printf("Sent values:     Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", beta[0], beta[1], beta[2], noise[0], noise[1], noise[2]);
01750    printf("Returned values:  Beta: %f X %f Y %f Z, White Noise: %f X %f Y %f Z\n", readback_beta[0], readback_beta[1], readback_beta[2],
01751                                                                                    readback_noise[0], readback_noise[1], readback_noise[2]);
01752   }
01753 
01754   printf("\n\nLoading the default gyro bias model values.\n\n");
01755 
01756   while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL, NULL) != MIP_INTERFACE_OK){}
01757  
01758   printf("\n\n");
01759   Sleep(1500);
01760  
01762   //Zero-Velocity Update Control
01764 
01765   printf("----------------------------------------------------------------------\n");
01766   printf("Setting Zero Velocity-Update threshold\n");
01767   printf("----------------------------------------------------------------------\n\n");
01768 
01769   zero_update_control.threshold = 0.1; // m/s
01770   zero_update_control.enable = 1; //enable zero-velocity update
01771 
01772   //Set ZUPT parameters
01773   while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01774 
01775   //Read back parameter settings  
01776   while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){}
01777 
01778   if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold)
01779    printf("ERROR configuring Zero Velocity Update.\n");
01780 
01781   printf("\n\nResetting Zero Velocity Update Control to default parameters.\n\n");
01782 
01783   //Reset to default value
01784   while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01785  
01786   printf("\n\n");
01787   Sleep(1500);
01788 
01790   //External Heading Update with Timestamp
01792 
01793   printf("----------------------------------------------------------------------\n");
01794   printf("Applying External Heading Update with Timestamp\n");
01795   printf("----------------------------------------------------------------------\n\n");
01796 
01797   //Set device to external heading update mode
01798   heading_source = 0x3;
01799 
01800   while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK){} 
01801 
01802   external_heading_with_time.gps_tow = 0.0;
01803   external_heading_with_time.gps_week_number = 0;
01804   external_heading_with_time.heading_angle_rads = 0.0;
01805   external_heading_with_time.heading_angle_sigma_rads = 0.05;
01806   external_heading_with_time.heading_type = 0x01;
01807 
01808   while(mip_filter_external_heading_update_with_time(&device_interface, &external_heading_with_time) != MIP_INTERFACE_OK){}
01809 
01810   //Resetting default heading update
01811   printf("\n\nResetting default heading update.\n\n");
01812   
01813   while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){} 
01814  
01815   printf("\n\n");
01816   Sleep(1500);
01817 
01819   //Angular Rate Zero Update Control
01821 
01822   printf("----------------------------------------------------------------------\n");
01823   printf("Setting Zero Angular-Rate-Update threshold\n");
01824   printf("----------------------------------------------------------------------\n\n");
01825 
01826   zero_update_control.threshold = 0.05; // rads/s
01827   zero_update_control.enable = 1; //enable zero-angular-rate update
01828 
01829   //Set ZUPT parameters
01830   while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01831 
01832   //Read back parameter settings  
01833   while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){}
01834 
01835   if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold)
01836    printf("ERROR configuring Zero Angular Rate Update.\n");
01837 
01838   printf("\n\nResetting Zero Angular Rate Update Control to default parameters.\n\n");
01839 
01840   //Reset to default value
01841   while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01842 
01843 
01844   printf("\n\n");
01845   Sleep(1500);
01846  
01847 
01849   //Tare Orientation
01851 
01852   printf("----------------------------------------------------------------------\n");
01853   printf("Performing Tare Orientation Command\n");
01854   printf("(This will only pass if the internal GPS solution is valid)\n");
01855   printf("----------------------------------------------------------------------\n\n");
01856 
01857 
01858   printf("Re-initializing filter (required for tare)\n\n");
01859 
01860   //Re-initialize the filter
01861   angles[0] = angles[1] = angles[2] = 0;
01862 
01863   while(mip_filter_set_init_attitude(&device_interface, angles) != MIP_INTERFACE_OK){}
01864 
01865   //Wait for Filter to re-establish running state
01866   Sleep(5000);
01867 
01868 
01869   //Cycle through axes combinations
01870   for(i=1; i<8; i++)
01871   {
01872 
01873    if(mip_filter_tare_orientation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK)
01874    {
01875     printf("ERROR: Failed Axis - ");
01876     
01877     if(i & FILTER_TARE_ROLL_AXIS)
01878      printf(" Roll Axis ");
01879 
01880     if(i & FILTER_TARE_PITCH_AXIS)
01881      printf(" Pitch Axis ");
01882 
01883     if(i & FILTER_TARE_YAW_AXIS)
01884      printf(" Yaw Axis ");
01885    }
01886    else
01887    {
01888     printf("Tare Configuration = %d\n", i);
01889 
01890         printf("Tared -");
01891 
01892         if(i & FILTER_TARE_ROLL_AXIS)
01893      printf(" Roll Axis ");
01894 
01895     if(i & FILTER_TARE_PITCH_AXIS)
01896      printf(" Pitch Axis ");
01897 
01898     if(i & FILTER_TARE_YAW_AXIS)
01899      printf(" Yaw Axis ");
01900 
01901         printf("\n\n");
01902    }
01903 
01904    Sleep(1000);
01905   } 
01906   
01907   printf("\n\nRestoring Orientation to default value.\n\n");
01908 
01909   //Restore to default value
01910   while(mip_filter_tare_orientation(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, 0) != MIP_INTERFACE_OK){}  
01911 
01912   printf("\n\n");
01913   Sleep(1500);
01914 
01916   //Commanded ZUPT
01918 
01919   printf("----------------------------------------------------------------------\n");
01920   printf("Performing Commanded Zero-Velocity Update\n");
01921   printf("----------------------------------------------------------------------\n\n");
01922 
01923   while(mip_filter_commanded_zero_velocity_update(&device_interface) != MIP_INTERFACE_OK){}
01924 
01925   printf("\n\n");
01926   Sleep(1500);
01927 
01929   //Commanded Zero Angular-Rate Update
01931 
01932   printf("----------------------------------------------------------------------\n");
01933   printf("Performing Commanded Zero-Angular-Rate Update\n");
01934   printf("----------------------------------------------------------------------\n\n");
01935 
01936   while(mip_filter_commanded_zero_angular_rate_update(&device_interface) != MIP_INTERFACE_OK){}
01937 
01938   printf("\n\n");
01939   Sleep(1500);
01940 
01942   //Set/Read the declination source
01944 
01945   printf("----------------------------------------------------------------------\n");
01946   printf("Cycle through available declination sources\n");
01947   printf("----------------------------------------------------------------------\n\n");
01948 
01949   //Loop through declination sources and set/read current source
01950   for(i=3; i>=1; i--)
01951   {
01952    //Set the source
01953    declination_source_command = i;
01954 
01955    while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_command) != MIP_INTERFACE_OK){}
01956 
01957    //Read back the declination source
01958    while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &declination_source_readback) != MIP_INTERFACE_OK){}
01959 
01960    if(declination_source_command == declination_source_readback)
01961    {
01962     printf("Declination source successfully set to %d\n", i);
01963    }
01964    else
01965    {
01966     printf("ERROR: Failed to set the declination source to %d!!!\n", i);
01967    }
01968   }
01969 
01970   printf("\n\nLoading the default declination source.\n\n");
01971 
01972   while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01973 
01974   printf("\n\n");
01975   Sleep(1500);
01976 
01977 
01979  //Set/Read the accel magnitude error adaptive measurement values
01981 
01982  printf("----------------------------------------------------------------------\n");
01983  printf("Setting the accel magnitude error adaptive measurement values\n");
01984  printf("----------------------------------------------------------------------\n\n");
01985 
01986  accel_magnitude_error_command.enable            = 0;
01987  accel_magnitude_error_command.low_pass_cutoff   = 10;
01988  accel_magnitude_error_command.min_1sigma        = 2.0;
01989  accel_magnitude_error_command.low_limit         = -2.0;
01990  accel_magnitude_error_command.high_limit        = 2.0;
01991  accel_magnitude_error_command.low_limit_1sigma  = 4.0;
01992  accel_magnitude_error_command.high_limit_1sigma = 4.0;
01993 
01994  while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK){}
01995 
01996  //Read back the accel magnitude error adaptive measurement values
01997  while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK){}
01998 
01999  if((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) &&
02000         (abs(accel_magnitude_error_command.low_pass_cutoff   - accel_magnitude_error_readback.low_pass_cutoff)   < 0.001) &&
02001         (abs(accel_magnitude_error_command.min_1sigma        - accel_magnitude_error_readback.min_1sigma)        < 0.001) &&
02002         (abs(accel_magnitude_error_command.low_limit         - accel_magnitude_error_readback.low_limit)         < 0.001) &&
02003         (abs(accel_magnitude_error_command.high_limit        - accel_magnitude_error_readback.high_limit)        < 0.001) &&
02004         (abs(accel_magnitude_error_command.low_limit_1sigma  - accel_magnitude_error_readback.low_limit_1sigma)  < 0.001) &&
02005         (abs(accel_magnitude_error_command.high_limit_1sigma - accel_magnitude_error_readback.high_limit_1sigma) < 0.001))
02006  {
02007   printf("accel magnitude error adaptive measurement values successfully set.\n");
02008  }
02009  else
02010  {
02011   printf("ERROR: Failed to set accel magnitude error adaptive measurement values!!!\n");
02012   printf("Sent values:     Enable: %i, Parameters: %f %f %f %f %f %f\n", accel_magnitude_error_command.enable,
02013                                                                          accel_magnitude_error_command.low_pass_cutoff, 
02014                                                                                                                                                  accel_magnitude_error_command.min_1sigma,
02015                                                                                                                                                  accel_magnitude_error_command.low_limit,
02016                                                                                                                                                  accel_magnitude_error_command.high_limit,
02017                                                                                                                                                  accel_magnitude_error_command.low_limit_1sigma,
02018                                                                                                                                                  accel_magnitude_error_command.high_limit_1sigma);
02019 
02020   printf("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", accel_magnitude_error_readback.enable,
02021                                                                          accel_magnitude_error_readback.low_pass_cutoff, 
02022                                                                                                                                                  accel_magnitude_error_readback.min_1sigma,
02023                                                                                                                                                  accel_magnitude_error_readback.low_limit,
02024                                                                                                                                                  accel_magnitude_error_readback.high_limit,
02025                                                                                                                                                  accel_magnitude_error_readback.low_limit_1sigma,
02026                                                                                                                                                  accel_magnitude_error_readback.high_limit_1sigma);
02027  }
02028 
02029   printf("\n\nLoading the default accel magnitude error adaptive measurement values.\n\n");
02030 
02031   while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02032 
02033  printf("\n\n");
02034  Sleep(1500);
02035 
02036 
02037  
02039  //Set/Read the mag magnitude error adaptive measurement values
02041 
02042  printf("----------------------------------------------------------------------\n");
02043  printf("Setting the mag magnitude error adaptive measurement values\n");
02044  printf("----------------------------------------------------------------------\n\n");
02045 
02046  mag_magnitude_error_command.enable            = 0;
02047  mag_magnitude_error_command.low_pass_cutoff   = 10;
02048  mag_magnitude_error_command.min_1sigma        = 0.1;
02049  mag_magnitude_error_command.low_limit         = -1.0;
02050  mag_magnitude_error_command.high_limit        = 1.0;
02051  mag_magnitude_error_command.low_limit_1sigma  = 1.0;
02052  mag_magnitude_error_command.high_limit_1sigma = 1.0;
02053 
02054  while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &mag_magnitude_error_command) != MIP_INTERFACE_OK){}
02055 
02056  //Read back the mag magnitude error adaptive measurement values
02057  while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK){}
02058 
02059  if((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) &&
02060         (abs(mag_magnitude_error_command.low_pass_cutoff   - mag_magnitude_error_readback.low_pass_cutoff)   < 0.001) &&
02061         (abs(mag_magnitude_error_command.min_1sigma        - mag_magnitude_error_readback.min_1sigma)        < 0.001) &&
02062         (abs(mag_magnitude_error_command.low_limit         - mag_magnitude_error_readback.low_limit)         < 0.001) &&
02063         (abs(mag_magnitude_error_command.high_limit        - mag_magnitude_error_readback.high_limit)        < 0.001) &&
02064         (abs(mag_magnitude_error_command.low_limit_1sigma  - mag_magnitude_error_readback.low_limit_1sigma)  < 0.001) &&
02065         (abs(mag_magnitude_error_command.high_limit_1sigma - mag_magnitude_error_readback.high_limit_1sigma) < 0.001))
02066  {
02067   printf("mag magnitude error adaptive measurement values successfully set.\n");
02068  }
02069  else
02070  {
02071   printf("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n");
02072   printf("Sent values:     Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_command.enable,
02073                                                                          mag_magnitude_error_command.low_pass_cutoff, 
02074                                                                                                                                                  mag_magnitude_error_command.min_1sigma,
02075                                                                                                                                                  mag_magnitude_error_command.low_limit,
02076                                                                                                                                                  mag_magnitude_error_command.high_limit,
02077                                                                                                                                                  mag_magnitude_error_command.low_limit_1sigma,
02078                                                                                                                                                  mag_magnitude_error_command.high_limit_1sigma);
02079 
02080   printf("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable,
02081                                                                          mag_magnitude_error_readback.low_pass_cutoff, 
02082                                                                                                                                                  mag_magnitude_error_readback.min_1sigma,
02083                                                                                                                                                  mag_magnitude_error_readback.low_limit,
02084                                                                                                                                                  mag_magnitude_error_readback.high_limit,
02085                                                                                                                                                  mag_magnitude_error_readback.low_limit_1sigma,
02086                                                                                                                                                  mag_magnitude_error_readback.high_limit_1sigma);
02087  }
02088 
02089   printf("\n\nLoading the default mag magnitude error adaptive measurement values.\n\n");
02090 
02091   while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02092 
02093   printf("\n\n");
02094   Sleep(1500);
02095 
02096 
02097  
02098 
02100  //Set/Read the mag dip angle error adaptive measurement values
02102 
02103  printf("----------------------------------------------------------------------\n");
02104  printf("Setting the mag dip angle error adaptive measurement values\n");
02105  printf("----------------------------------------------------------------------\n\n");
02106 
02107  mag_dip_angle_error_command.enable            = 0;
02108  mag_dip_angle_error_command.low_pass_cutoff   = 10;
02109  mag_dip_angle_error_command.min_1sigma        = 0.1;
02110  mag_dip_angle_error_command.high_limit        = 90.0*3.14/180.0;
02111  mag_dip_angle_error_command.high_limit_1sigma = 2.0;
02112 
02113  while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK){}
02114 
02115  //Read back the mag magnitude error adaptive measurement values
02116  while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK){}
02117 
02118  if((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) &&
02119         (abs(mag_dip_angle_error_command.low_pass_cutoff   - mag_dip_angle_error_readback.low_pass_cutoff)   < 0.001) &&
02120         (abs(mag_dip_angle_error_command.min_1sigma        - mag_dip_angle_error_readback.min_1sigma)        < 0.001) &&
02121         (abs(mag_dip_angle_error_command.high_limit        - mag_dip_angle_error_readback.high_limit)        < 0.001) &&
02122         (abs(mag_dip_angle_error_command.high_limit_1sigma - mag_dip_angle_error_readback.high_limit_1sigma) < 0.001))
02123  {
02124   printf("mag dip angle error adaptive measurement values successfully set.\n");
02125  }
02126  else
02127  {
02128   printf("ERROR: Failed to set mag dip angle error adaptive measurement values!!!\n");
02129   printf("Sent values:     Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_command.enable,
02130                                                                    mag_dip_angle_error_command.low_pass_cutoff, 
02131                                                                                                                                    mag_dip_angle_error_command.min_1sigma,
02132                                                                                                                                    mag_dip_angle_error_command.high_limit,
02133                                                                                                                                    mag_dip_angle_error_command.high_limit_1sigma);
02134 
02135   printf("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable,
02136                                                                    mag_dip_angle_error_readback.low_pass_cutoff, 
02137                                                                                                                                    mag_dip_angle_error_readback.min_1sigma,
02138                                                                                                                                    mag_dip_angle_error_readback.high_limit,
02139                                                                                                                                    mag_dip_angle_error_readback.high_limit_1sigma);
02140  }
02141 
02142   printf("\n\nLoading the default mag magnitude error adaptive measurement values.\n\n");
02143 
02144   while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02145 
02146   printf("\n\n");
02147   Sleep(1500);
02148 
02149 
02150 
02151 
02153  //
02154  // Streaming Data Tests
02155  //
02157  
02158 
02159 
02161   //Setup the GX4-45 dataset callbacks
02163 
02164   if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_FILTER_DATA_SET, NULL, &filter_packet_callback) != MIP_INTERFACE_OK)
02165    return -1;
02166   
02167   if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_AHRS_DATA_SET, NULL, &ahrs_packet_callback) != MIP_INTERFACE_OK)
02168    return -1;
02169 
02170   if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_GPS_DATA_SET, NULL, &gps_packet_callback) != MIP_INTERFACE_OK)
02171    return -1;
02172 
02173   //Enable the output of data statistics
02174   enable_data_stats_output = 1;
02175 
02177   //Setup the AHRS datastream format
02179 
02180   printf("----------------------------------------------------------------------\n");
02181   printf("Setting the AHRS message format\n");
02182   printf("----------------------------------------------------------------------\n\n");
02183 
02184   data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED;
02185   data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED;
02186 
02187   data_stream_format_decimation[0]  = 0x32;
02188   data_stream_format_decimation[1]  = 0x32;
02189 
02190   data_stream_format_num_entries = 2;
02191 
02192   while(mip_3dm_cmd_ahrs_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries, data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
02193  
02194   printf("\n\n");
02195   Sleep(1500);
02196 
02198   //Poll the AHRS data
02200 
02201   printf("----------------------------------------------------------------------\n");
02202   printf("Polling AHRS Data.\n");
02203   printf("----------------------------------------------------------------------\n\n");
02204 
02205   while(mip_3dm_cmd_poll_ahrs(&device_interface, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK){}
02206  
02207   printf("\n\n");
02208   Sleep(1500);
02209 
02211   //Setup the GPS datastream format
02213 
02214   printf("----------------------------------------------------------------------\n");
02215   printf("Setting the GPS datastream format\n");
02216   printf("----------------------------------------------------------------------\n\n");
02217 
02218   data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS;
02219   data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY;
02220   data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME;
02221 
02222   data_stream_format_decimation[0]  = 0x04;
02223   data_stream_format_decimation[1]  = 0x04;
02224   data_stream_format_decimation[2]  = 0x04;
02225   
02226   data_stream_format_num_entries = 3;
02227 
02228   while(mip_3dm_cmd_gps_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
02229                                        data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
02230 
02231   printf("\n\n");
02232   Sleep(1500);
02233 
02235   //Poll the GPS data
02237 
02238   printf("----------------------------------------------------------------------\n");
02239   printf("Polling GPS Data.\n");
02240   printf("----------------------------------------------------------------------\n\n");
02241 
02242   while(mip_3dm_cmd_poll_gps(&device_interface, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK){}
02243 
02244   printf("\n\n");
02245   Sleep(1500);
02246 
02248   //Setup the FILTER datastream format
02250 
02251   printf("----------------------------------------------------------------------\n");
02252   printf("Setting the Estimation Filter datastream format\n");
02253   printf("----------------------------------------------------------------------\n\n");
02254 
02255   data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS;
02256   data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL;
02257   data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES;
02258 
02259   data_stream_format_decimation[0]  = 0x32;
02260   data_stream_format_decimation[1]  = 0x32;
02261   data_stream_format_decimation[2]  = 0x32;
02262 
02263   data_stream_format_num_entries = 3;
02264 
02265   while(mip_3dm_cmd_filter_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
02266                                        data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
02267  
02268   printf("\n\n");
02269   Sleep(1500);
02270 
02272   //Poll the Estimation Filter data
02274 
02275   printf("----------------------------------------------------------------------\n");
02276   printf("Polling Estimation Filter Data.\n");
02277   printf("----------------------------------------------------------------------\n\n");
02278 
02279   while(mip_3dm_cmd_poll_filter(&device_interface, MIP_3DM_POLLING_ENABLE_ACK_NACK, data_stream_format_num_entries, data_stream_format_descriptors) != MIP_INTERFACE_OK){}
02280 
02281   printf("\n\n");
02282   Sleep(1500);
02283  }
02284  else
02285  {
02286   printf("ERROR: Standard mode not established\n\n");
02287  }
02288 
02289 
02290  //Disable the output of data statistics
02291  enable_data_stats_output = 0;
02292 
02294  //Enable the AHRS datastream
02296 
02297  printf("\n----------------------------------------------------------------------\n");
02298  printf("Enable the AHRS datastream\n");
02299  printf("----------------------------------------------------------------------\n\n");
02300 
02301  enable = 0x01;
02302 
02303  while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02304  
02305  printf("\n\n");
02306  Sleep(1500);
02307 
02309  //Enable the FILTER datastream
02311 
02312  printf("----------------------------------------------------------------------\n");
02313  printf("Enable the FILTER datastream\n");
02314  printf("----------------------------------------------------------------------\n\n");
02315 
02316  enable = 0x01;
02317 
02318  while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02319 
02320  printf("\n\n");
02321  Sleep(1500);
02322 
02324  //Enable the GPS datastream
02326 
02327  printf("----------------------------------------------------------------------\n");
02328  printf("Enable the GPS datastream\n");
02329  printf("----------------------------------------------------------------------\n");
02330 
02331  enable = 0x01;
02332 
02333  while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02334 
02335  printf("\n\n");
02336  Sleep(1500);
02337 
02339  //Wait for packets to arrive
02341 
02342  printf("----------------------------------------------------------------------\n");
02343  printf("Processing incoming packets\n");
02344  printf("----------------------------------------------------------------------\n\n\n");
02345 
02346  //Enable the output of data statistics
02347  enable_data_stats_output = 1;
02348 
02349  while(1)
02350  {
02351   //Update the parser (this function reads the port and parses the bytes
02352   mip_interface_update(&device_interface);
02353 
02354   //Be nice to other programs
02355   Sleep(1);
02356  }
02357 }
02358 
02359 
02360 
02362 //
02363 // FILTER Packet Callback
02364 //
02366 
02367 void filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02368 {
02369  mip_field_header *field_header;
02370  u8               *field_data;
02371  u16              field_offset = 0;
02372 
02373  //The packet callback can have several types, process them all
02374  switch(callback_type)
02375  {
02377   //Handle valid packets
02379 
02380   case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02381   {
02382    filter_valid_packet_count++;
02383 
02385    //Loop through all of the data fields
02387 
02388    while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02389    {
02390 
02392     // Decode the field
02394 
02395     switch(field_header->descriptor)
02396     {
02398      // Estimated LLH Position
02400 
02401      case MIP_FILTER_DATA_LLH_POS:
02402      {
02403       memcpy(&curr_filter_pos, field_data, sizeof(mip_filter_llh_pos));
02404 
02405       //For little-endian targets, byteswap the data field
02406       mip_filter_llh_pos_byteswap(&curr_filter_pos);
02407 
02408      }break;
02409 
02411      // Estimated NED Velocity
02413 
02414      case MIP_FILTER_DATA_NED_VEL:
02415      {
02416       memcpy(&curr_filter_vel, field_data, sizeof(mip_filter_ned_velocity));
02417 
02418       //For little-endian targets, byteswap the data field
02419       mip_filter_ned_velocity_byteswap(&curr_filter_vel);
02420 
02421      }break;
02422 
02424      // Estimated Attitude, Euler Angles
02426 
02427      case MIP_FILTER_DATA_ATT_EULER_ANGLES:
02428      {
02429       memcpy(&curr_filter_angles, field_data, sizeof(mip_filter_attitude_euler_angles));
02430 
02431       //For little-endian targets, byteswap the data field
02432       mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles);
02433 
02434      }break;
02435 
02436      default: break;
02437     }
02438    }
02439   }break;
02440 
02441 
02443   //Handle checksum error packets
02445 
02446   case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02447   {
02448    filter_checksum_error_packet_count++;
02449   }break;
02450 
02452   //Handle timeout packets
02454 
02455   case MIP_INTERFACE_CALLBACK_TIMEOUT:
02456   {
02457    filter_timeout_packet_count++;
02458   }break;
02459   default: break;
02460  }
02461 
02462  print_packet_stats();
02463 }
02464 
02465 
02467 //
02468 // AHRS Packet Callback
02469 //
02471 
02472 void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02473 {
02474  mip_field_header *field_header;
02475  u8               *field_data;
02476  u16              field_offset = 0;
02477 
02478  //The packet callback can have several types, process them all
02479  switch(callback_type)
02480  {
02482   //Handle valid packets
02484 
02485   case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02486   {
02487    ahrs_valid_packet_count++;
02488 
02490    //Loop through all of the data fields
02492 
02493    while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02494    {
02495 
02497     // Decode the field
02499 
02500     switch(field_header->descriptor)
02501     {
02503      // Scaled Accelerometer
02505 
02506      case MIP_AHRS_DATA_ACCEL_SCALED:
02507      {
02508       memcpy(&curr_ahrs_accel, field_data, sizeof(mip_ahrs_scaled_accel));
02509 
02510       //For little-endian targets, byteswap the data field
02511       mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel);
02512 
02513      }break;
02514 
02516      // Scaled Gyro
02518 
02519      case MIP_AHRS_DATA_GYRO_SCALED:
02520      {
02521       memcpy(&curr_ahrs_gyro, field_data, sizeof(mip_ahrs_scaled_gyro));
02522 
02523       //For little-endian targets, byteswap the data field
02524       mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro);
02525 
02526      }break;
02527 
02529      // Scaled Magnetometer
02531 
02532      case MIP_AHRS_DATA_MAG_SCALED:
02533      {
02534       memcpy(&curr_ahrs_mag, field_data, sizeof(mip_ahrs_scaled_mag));
02535 
02536       //For little-endian targets, byteswap the data field
02537       mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag);
02538 
02539      }break;
02540 
02541      default: break;
02542     }
02543    }
02544   }break;
02545 
02547   //Handle checksum error packets
02549 
02550   case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02551   {
02552    ahrs_checksum_error_packet_count++;
02553   }break;
02554 
02556   //Handle timeout packets
02558 
02559   case MIP_INTERFACE_CALLBACK_TIMEOUT:
02560   {
02561    ahrs_timeout_packet_count++;
02562   }break;
02563   default: break;
02564  }
02565 
02566  print_packet_stats();
02567 }
02568 
02569 
02571 //
02572 // GPS Packet Callback
02573 //
02575 
02576 void gps_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02577 {
02578  mip_field_header *field_header;
02579  u8               *field_data;
02580  u16              field_offset = 0;
02581 
02582  //The packet callback can have several types, process them all
02583  switch(callback_type)
02584  {
02586   //Handle valid packets
02588 
02589   case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02590   {
02591    gps_valid_packet_count++;
02592 
02594    //Loop through all of the data fields
02596 
02597    while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02598    {
02599 
02601     // Decode the field
02603 
02604     switch(field_header->descriptor)
02605     {
02607      // LLH Position
02609 
02610      case MIP_GPS_DATA_LLH_POS:
02611      {
02612       memcpy(&curr_llh_pos, field_data, sizeof(mip_gps_llh_pos));
02613 
02614       //For little-endian targets, byteswap the data field
02615       mip_gps_llh_pos_byteswap(&curr_llh_pos);
02616 
02617      }break;
02618 
02620      // NED Velocity
02622 
02623      case MIP_GPS_DATA_NED_VELOCITY:
02624      {
02625       memcpy(&curr_ned_vel, field_data, sizeof(mip_gps_ned_vel));
02626 
02627       //For little-endian targets, byteswap the data field
02628       mip_gps_ned_vel_byteswap(&curr_ned_vel);
02629 
02630      }break;
02631 
02633      // GPS Time
02635 
02636      case MIP_GPS_DATA_GPS_TIME:
02637      {
02638       memcpy(&curr_gps_time, field_data, sizeof(mip_gps_time));
02639 
02640       //For little-endian targets, byteswap the data field
02641       mip_gps_time_byteswap(&curr_gps_time);
02642 
02643      }break;
02644 
02645      default: break;
02646     }
02647    }
02648   }break;
02649 
02650 
02652   //Handle checksum error packets
02654 
02655   case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02656   {
02657    gps_checksum_error_packet_count++;
02658   }break;
02659 
02661   //Handle timeout packets
02663 
02664   case MIP_INTERFACE_CALLBACK_TIMEOUT:
02665   {
02666    gps_timeout_packet_count++;
02667   }break;
02668   default: break;
02669  }
02670 
02671  print_packet_stats();
02672 }
02673 
02674 
02676 //
02677 // Print Command-Line Help
02678 //
02680 
02681 void print_command_line_usage()
02682 {
02683  printf("\n\n");
02684  printf("Usage:\n");
02685  printf("-----------------------------------------------------------------------\n\n");
02686 
02687  printf("   GX4-45_Test [com_port_num] [baudrate]\n");
02688  printf("\n\n");
02689  printf("   Example: \"GX4-45_Test 1 115200\", Opens a connection to the \n");
02690  printf("             GX4-45 on COM1, with a baudrate of 115200.\n");
02691  printf("\n\n");
02692  printf("   [ ] - required command input.\n");
02693  printf("\n-----------------------------------------------------------------------\n");
02694  printf("\n\n");
02695 }
02696 
02697 
02699 //
02700 // Print Header
02701 //
02703 
02704 void print_header()
02705 {
02706  printf("\n");
02707  printf("GX4-45 Test Program\n");
02708  printf("Copyright 2013. LORD Microstrain\n\n");
02709 }
02710 
02711 
02713 //
02714 // Print Packet Statistics
02715 //
02717 
02718 void print_packet_stats()
02719 {
02720  if(enable_data_stats_output)
02721  {
02722   printf("\r%u FILTER (%u errors)    %u AHRS (%u errors)    %u GPS (%u errors) Packets", filter_valid_packet_count,  filter_timeout_packet_count + filter_checksum_error_packet_count,
02723                                                                                       ahrs_valid_packet_count, ahrs_timeout_packet_count + ahrs_checksum_error_packet_count,
02724                                                                                       gps_valid_packet_count,  gps_timeout_packet_count + gps_checksum_error_packet_count);
02725  }
02726 }
02727 
02729 //
02730 // Device specific Status Acquisition Routines
02731 //
02733 
02735 //                                                                              
02738 //                                                                              
02741 //                                                                              
02748 //                                                                              
02751 //                                                                              
02756 
02757 u16 mip_3dm_cmd_hw_specific_imu_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer)
02758 {
02759 
02760  gx4_imu_basic_status_field *basic_ptr;
02761  gx4_imu_diagnostic_device_status_field *diagnostic_ptr;
02762  u16 response_size = MIP_FIELD_HEADER_SIZE;
02763 
02764  if(status_selector == GX4_IMU_BASIC_STATUS_SEL)                                    
02765   response_size += sizeof(gx4_imu_basic_status_field);                               
02766  else if(status_selector == GX4_IMU_DIAGNOSTICS_STATUS_SEL)                         
02767   response_size += sizeof(gx4_imu_diagnostic_device_status_field); 
02768 
02769  while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){}
02770 
02771  if(status_selector == GX4_IMU_BASIC_STATUS_SEL){
02772 
02773   if(response_size != sizeof(gx4_imu_basic_status_field))
02774    return MIP_INTERFACE_ERROR;
02775   else if(MIP_SDK_CONFIG_BYTESWAP){
02776  
02777    basic_ptr = (gx4_imu_basic_status_field *)response_buffer;
02778 
02779    byteswap_inplace(&basic_ptr->device_model, sizeof(basic_ptr->device_model));
02780    byteswap_inplace(&basic_ptr->status_flags, sizeof(basic_ptr->status_flags));
02781    byteswap_inplace(&basic_ptr->system_timer_ms, sizeof(basic_ptr->system_timer_ms));
02782 
02783   }
02784 
02785  }else if(status_selector == GX4_IMU_DIAGNOSTICS_STATUS_SEL){
02786 
02787   if(response_size != sizeof(gx4_imu_diagnostic_device_status_field))
02788    return MIP_INTERFACE_ERROR;
02789   else if(MIP_SDK_CONFIG_BYTESWAP){
02790  
02791    diagnostic_ptr = (gx4_imu_diagnostic_device_status_field *)response_buffer;
02792 
02793    byteswap_inplace(&diagnostic_ptr->device_model, sizeof(diagnostic_ptr->device_model));
02794    byteswap_inplace(&diagnostic_ptr->status_flags, sizeof(diagnostic_ptr->status_flags));
02795    byteswap_inplace(&diagnostic_ptr->system_timer_ms, sizeof(diagnostic_ptr->system_timer_ms));
02796    byteswap_inplace(&diagnostic_ptr->gyro_range, sizeof(diagnostic_ptr->gyro_range));
02797    byteswap_inplace(&diagnostic_ptr->mag_range, sizeof(diagnostic_ptr->mag_range));
02798    byteswap_inplace(&diagnostic_ptr->pressure_range, sizeof(diagnostic_ptr->pressure_range));
02799    byteswap_inplace(&diagnostic_ptr->temp_degc, sizeof(diagnostic_ptr->temp_degc));
02800    byteswap_inplace(&diagnostic_ptr->last_temp_read_ms, sizeof(diagnostic_ptr->last_temp_read_ms));
02801    byteswap_inplace(&diagnostic_ptr->num_gps_pps_triggers, sizeof(diagnostic_ptr->num_gps_pps_triggers));
02802    byteswap_inplace(&diagnostic_ptr->last_gps_pps_trigger_ms, sizeof(diagnostic_ptr->last_gps_pps_trigger_ms));
02803    byteswap_inplace(&diagnostic_ptr->dropped_packets, sizeof(diagnostic_ptr->dropped_packets));
02804    byteswap_inplace(&diagnostic_ptr->com_port_bytes_written, sizeof(diagnostic_ptr->com_port_bytes_written));
02805    byteswap_inplace(&diagnostic_ptr->com_port_bytes_read, sizeof(diagnostic_ptr->com_port_bytes_read));
02806    byteswap_inplace(&diagnostic_ptr->com_port_write_overruns, sizeof(diagnostic_ptr->com_port_write_overruns));
02807    byteswap_inplace(&diagnostic_ptr->com_port_read_overruns, sizeof(diagnostic_ptr->com_port_read_overruns));
02808 
02809   }
02810 
02811  }
02812  else
02813   return MIP_INTERFACE_ERROR;
02814 
02815  return MIP_INTERFACE_OK;
02816 
02817 }
02818 
02819 
02821 //                                                                              
02824 //                                                                              
02827 //                                                                              
02834 //                                                                              
02837 //                                                                              
02841     
02842 u16 mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer)
02843 {
02844 
02845  gx4_45_basic_status_field *basic_ptr;
02846  gx4_45_diagnostic_device_status_field *diagnostic_ptr;
02847  u16 response_size = MIP_FIELD_HEADER_SIZE;
02848 
02849  if(status_selector == GX4_45_BASIC_STATUS_SEL)                                    
02850   response_size += sizeof(gx4_45_basic_status_field);                               
02851  else if(status_selector == GX4_45_DIAGNOSTICS_STATUS_SEL)                         
02852   response_size += sizeof(gx4_45_diagnostic_device_status_field); 
02853 
02854  while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){}
02855 
02856  if(status_selector == GX4_45_BASIC_STATUS_SEL)
02857  {
02858 
02859   if(response_size != sizeof(gx4_45_basic_status_field))
02860    return MIP_INTERFACE_ERROR;
02861   else if(MIP_SDK_CONFIG_BYTESWAP){
02862 
02863    basic_ptr = (gx4_45_basic_status_field *)response_buffer;
02864 
02865    byteswap_inplace(&basic_ptr->device_model, sizeof(basic_ptr->device_model));
02866    byteswap_inplace(&basic_ptr->status_flags, sizeof(basic_ptr->status_flags));
02867    byteswap_inplace(&basic_ptr->system_state, sizeof(basic_ptr->system_state));
02868    byteswap_inplace(&basic_ptr->system_timer_ms, sizeof(basic_ptr->system_timer_ms));
02869 
02870   }
02871   
02872  }
02873  else if(status_selector == GX4_45_DIAGNOSTICS_STATUS_SEL)
02874  {
02875 
02876   if(response_size != sizeof(gx4_45_diagnostic_device_status_field))
02877    return MIP_INTERFACE_ERROR;
02878   else if(MIP_SDK_CONFIG_BYTESWAP){
02879 
02880    diagnostic_ptr = (gx4_45_diagnostic_device_status_field *)response_buffer;
02881 
02882    byteswap_inplace(&diagnostic_ptr->device_model, sizeof(diagnostic_ptr->device_model));
02883    byteswap_inplace(&diagnostic_ptr->status_flags, sizeof(diagnostic_ptr->status_flags));
02884    byteswap_inplace(&diagnostic_ptr->system_state, sizeof(diagnostic_ptr->system_state));
02885    byteswap_inplace(&diagnostic_ptr->system_timer_ms, sizeof(diagnostic_ptr->system_timer_ms));
02886    byteswap_inplace(&diagnostic_ptr->num_gps_pps_triggers, sizeof(diagnostic_ptr->num_gps_pps_triggers));
02887    byteswap_inplace(&diagnostic_ptr->last_gps_pps_trigger_ms, sizeof(diagnostic_ptr->last_gps_pps_trigger_ms));
02888    byteswap_inplace(&diagnostic_ptr->imu_dropped_packets, sizeof(diagnostic_ptr->imu_dropped_packets));
02889    byteswap_inplace(&diagnostic_ptr->gps_dropped_packets, sizeof(diagnostic_ptr->gps_dropped_packets));
02890    byteswap_inplace(&diagnostic_ptr->filter_dropped_packets, sizeof(diagnostic_ptr->filter_dropped_packets));
02891    byteswap_inplace(&diagnostic_ptr->com1_port_bytes_written, sizeof(diagnostic_ptr->com1_port_bytes_written));
02892    byteswap_inplace(&diagnostic_ptr->com1_port_bytes_read, sizeof(diagnostic_ptr->com1_port_bytes_read));
02893    byteswap_inplace(&diagnostic_ptr->com1_port_write_overruns, sizeof(diagnostic_ptr->com1_port_write_overruns));
02894    byteswap_inplace(&diagnostic_ptr->com1_port_read_overruns, sizeof(diagnostic_ptr->com1_port_read_overruns));
02895    byteswap_inplace(&diagnostic_ptr->imu_parser_errors, sizeof(diagnostic_ptr->imu_parser_errors));
02896    byteswap_inplace(&diagnostic_ptr->imu_message_count, sizeof(diagnostic_ptr->imu_message_count));
02897    byteswap_inplace(&diagnostic_ptr->imu_last_message_ms, sizeof(diagnostic_ptr->imu_last_message_ms));
02898    byteswap_inplace(&diagnostic_ptr->gps_parser_errors, sizeof(diagnostic_ptr->gps_parser_errors));
02899    byteswap_inplace(&diagnostic_ptr->gps_message_count, sizeof(diagnostic_ptr->gps_message_count));
02900    byteswap_inplace(&diagnostic_ptr->gps_last_message_ms, sizeof(diagnostic_ptr->gps_last_message_ms));
02901   }
02902  }
02903  else
02904   return MIP_INTERFACE_ERROR;
02905 
02906  return MIP_INTERFACE_OK;
02907 
02908 }


microstrain_3dm_gx5_45
Author(s): Brian Bingham
autogenerated on Tue Apr 18 2017 02:59:09