00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00020
00023
00033
00035
00036
00038
00039
00040
00042
00043 #include "microstrain_3dm_gx5_45/GX4-45_Test.h"
00044
00046
00047
00048
00050
00052
00053
00054
00056
00057 u8 enable_data_stats_output = 0;
00058
00059
00060 mip_interface device_interface;
00061
00062
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
00076
00077
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
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
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
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
00161
00162 if(argc != NUM_COMMAND_LINE_ARGUMENTS)
00163 {
00164 print_command_line_usage();
00165 return -1;
00166 }
00167
00168
00169 com_port = atoi(argv[1]);
00170 baudrate = atoi(argv[2]);
00171
00173
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
00182
00183
00185
00186 com_mode = MIP_SDK_GX4_45_IMU_DIRECT_MODE;
00187
00189
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
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
00215
00217
00219
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
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
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
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
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
00318
00320
00322
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
00339
00340
00341 printf("----------------------------------------------------------------------\n");
00342 printf("Requesting BASIC Status Report:\n");
00343 printf("----------------------------------------------------------------------\n\n");
00344
00345
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
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
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
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
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
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;
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
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
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
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
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
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
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
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
00638 while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){}
00639
00640
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
00653
00655
00657
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
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
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
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
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
00758
00760
00762
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
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
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
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
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
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
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;
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
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
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
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
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
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
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
01054
01055 printf("----------------------------------------------------------------------\n");
01056 printf("Requesting BASIC Status Report\n");
01057 printf("----------------------------------------------------------------------\n\n");
01058
01059
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
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
01116
01118
01119
01121
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
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
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
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
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
01190
01191 printf("----------------------------------------------------------------------\n");
01192 printf("Cycle through available Vehicle Dynamics Modes\n");
01193 printf("----------------------------------------------------------------------\n\n");
01194
01195
01196 for(i=3; i>=1; i--)
01197 {
01198
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
01204 readback_dynamics_mode = 0;
01205
01206
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
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
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
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
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
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
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
01341
01342 printf("----------------------------------------------------------------------\n");
01343 printf("Cycling through Estimation Control Flags\n");
01344 printf("----------------------------------------------------------------------\n\n");
01345
01346
01347 for(j=0; j < 0x0020 ; j++)
01348 {
01349
01350 estimation_control = (0xFFFF & ~j);
01351
01352
01353 while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK){}
01354
01355
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
01389
01390 printf("----------------------------------------------------------------------\n");
01391 printf("Cycle through available GPS sources\n");
01392 printf("----------------------------------------------------------------------\n\n");
01393
01394
01395 for(i=2; i>=1; i--)
01396 {
01397
01398 gps_source = i;
01399
01400 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01401
01402
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
01425
01426
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
01436 external_gps_update.tow = 0;
01437 external_gps_update.week_number = 0;
01438 external_gps_update.pos[0] = 44.43753433;
01439 external_gps_update.pos[1] = -73.10612488;
01440 external_gps_update.pos[2] = 134.029999;
01441 external_gps_update.vel[0] = 0.0;
01442 external_gps_update.vel[1] = 0.0;
01443 external_gps_update.vel[2] = 0.0;
01444 external_gps_update.pos_1sigma[0] = 0.1;
01445 external_gps_update.pos_1sigma[1] = 0.1;
01446 external_gps_update.pos_1sigma[2] = 0.1;
01447 external_gps_update.vel_1sigma[0] = 0.1;
01448 external_gps_update.vel_1sigma[1] = 0.1;
01449 external_gps_update.vel_1sigma[2] = 0.1;
01450
01451 while(mip_filter_external_gps_update(&device_interface, &external_gps_update) != MIP_INTERFACE_OK){}
01452
01453
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
01464
01465 printf("----------------------------------------------------------------------\n");
01466 printf("Performing External Heading Update with externally supplied heading information\n");
01467 printf("----------------------------------------------------------------------\n\n");
01468
01469
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;
01475 external_heading_update.heading_angle_1sigma = 0.01;
01476 external_heading_update.type = 0x1;
01477
01478 while(mip_filter_external_heading_update(&device_interface, &external_heading_update) != MIP_INTERFACE_OK){}
01479
01480
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
01490
01491 printf("----------------------------------------------------------------------\n");
01492 printf("Cycle through available Heading sources\n");
01493 printf("----------------------------------------------------------------------\n\n");
01494
01495
01496 for(i=3; i>=0; i--)
01497 {
01498
01499 heading_source = i;
01500
01501 mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source);
01502
01503
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
01526
01527 printf("----------------------------------------------------------------------\n");
01528 printf("Cycle through available auto-init values\n");
01529 printf("----------------------------------------------------------------------\n\n");
01530
01531
01532 for(i=1; i>=0; i--)
01533 {
01534
01535 auto_init = i;
01536
01537 while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &auto_init) != MIP_INTERFACE_OK){}
01538
01539
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
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
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
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
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
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
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
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
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
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
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
01764
01765 printf("----------------------------------------------------------------------\n");
01766 printf("Setting Zero Velocity-Update threshold\n");
01767 printf("----------------------------------------------------------------------\n\n");
01768
01769 zero_update_control.threshold = 0.1;
01770 zero_update_control.enable = 1;
01771
01772
01773 while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01774
01775
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
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
01792
01793 printf("----------------------------------------------------------------------\n");
01794 printf("Applying External Heading Update with Timestamp\n");
01795 printf("----------------------------------------------------------------------\n\n");
01796
01797
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
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
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;
01827 zero_update_control.enable = 1;
01828
01829
01830 while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01831
01832
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
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
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
01861 angles[0] = angles[1] = angles[2] = 0;
01862
01863 while(mip_filter_set_init_attitude(&device_interface, angles) != MIP_INTERFACE_OK){}
01864
01865
01866 Sleep(5000);
01867
01868
01869
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
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
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
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
01944
01945 printf("----------------------------------------------------------------------\n");
01946 printf("Cycle through available declination sources\n");
01947 printf("----------------------------------------------------------------------\n\n");
01948
01949
01950 for(i=3; i>=1; i--)
01951 {
01952
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
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
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
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
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
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
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
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
02155
02157
02158
02159
02161
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
02174 enable_data_stats_output = 1;
02175
02177
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
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
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
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
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
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
02291 enable_data_stats_output = 0;
02292
02294
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
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
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
02341
02342 printf("----------------------------------------------------------------------\n");
02343 printf("Processing incoming packets\n");
02344 printf("----------------------------------------------------------------------\n\n\n");
02345
02346
02347 enable_data_stats_output = 1;
02348
02349 while(1)
02350 {
02351
02352 mip_interface_update(&device_interface);
02353
02354
02355 Sleep(1);
02356 }
02357 }
02358
02359
02360
02362
02363
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
02374 switch(callback_type)
02375 {
02377
02379
02380 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02381 {
02382 filter_valid_packet_count++;
02383
02385
02387
02388 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02389 {
02390
02392
02394
02395 switch(field_header->descriptor)
02396 {
02398
02400
02401 case MIP_FILTER_DATA_LLH_POS:
02402 {
02403 memcpy(&curr_filter_pos, field_data, sizeof(mip_filter_llh_pos));
02404
02405
02406 mip_filter_llh_pos_byteswap(&curr_filter_pos);
02407
02408 }break;
02409
02411
02413
02414 case MIP_FILTER_DATA_NED_VEL:
02415 {
02416 memcpy(&curr_filter_vel, field_data, sizeof(mip_filter_ned_velocity));
02417
02418
02419 mip_filter_ned_velocity_byteswap(&curr_filter_vel);
02420
02421 }break;
02422
02424
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
02432 mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles);
02433
02434 }break;
02435
02436 default: break;
02437 }
02438 }
02439 }break;
02440
02441
02443
02445
02446 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02447 {
02448 filter_checksum_error_packet_count++;
02449 }break;
02450
02452
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
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
02479 switch(callback_type)
02480 {
02482
02484
02485 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02486 {
02487 ahrs_valid_packet_count++;
02488
02490
02492
02493 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02494 {
02495
02497
02499
02500 switch(field_header->descriptor)
02501 {
02503
02505
02506 case MIP_AHRS_DATA_ACCEL_SCALED:
02507 {
02508 memcpy(&curr_ahrs_accel, field_data, sizeof(mip_ahrs_scaled_accel));
02509
02510
02511 mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel);
02512
02513 }break;
02514
02516
02518
02519 case MIP_AHRS_DATA_GYRO_SCALED:
02520 {
02521 memcpy(&curr_ahrs_gyro, field_data, sizeof(mip_ahrs_scaled_gyro));
02522
02523
02524 mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro);
02525
02526 }break;
02527
02529
02531
02532 case MIP_AHRS_DATA_MAG_SCALED:
02533 {
02534 memcpy(&curr_ahrs_mag, field_data, sizeof(mip_ahrs_scaled_mag));
02535
02536
02537 mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag);
02538
02539 }break;
02540
02541 default: break;
02542 }
02543 }
02544 }break;
02545
02547
02549
02550 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02551 {
02552 ahrs_checksum_error_packet_count++;
02553 }break;
02554
02556
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
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
02583 switch(callback_type)
02584 {
02586
02588
02589 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02590 {
02591 gps_valid_packet_count++;
02592
02594
02596
02597 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02598 {
02599
02601
02603
02604 switch(field_header->descriptor)
02605 {
02607
02609
02610 case MIP_GPS_DATA_LLH_POS:
02611 {
02612 memcpy(&curr_llh_pos, field_data, sizeof(mip_gps_llh_pos));
02613
02614
02615 mip_gps_llh_pos_byteswap(&curr_llh_pos);
02616
02617 }break;
02618
02620
02622
02623 case MIP_GPS_DATA_NED_VELOCITY:
02624 {
02625 memcpy(&curr_ned_vel, field_data, sizeof(mip_gps_ned_vel));
02626
02627
02628 mip_gps_ned_vel_byteswap(&curr_ned_vel);
02629
02630 }break;
02631
02633
02635
02636 case MIP_GPS_DATA_GPS_TIME:
02637 {
02638 memcpy(&curr_gps_time, field_data, sizeof(mip_gps_time));
02639
02640
02641 mip_gps_time_byteswap(&curr_gps_time);
02642
02643 }break;
02644
02645 default: break;
02646 }
02647 }
02648 }break;
02649
02650
02652
02654
02655 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02656 {
02657 gps_checksum_error_packet_count++;
02658 }break;
02659
02661
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
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
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
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
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 }