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 "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 const char* portstr = "/dev/ttyUSB0";
00169
00170 portstr = argv[1];
00171
00172 baudrate = atoi(argv[2]);
00173
00175
00177 printf("Attempting to open interface on COM port %d \n",com_port);
00178 if(mip_interface_init(portstr, baudrate, &device_interface, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK)
00179 return -1;
00180
00181
00183
00184
00186
00187 com_mode = MIP_SDK_GX4_45_IMU_DIRECT_MODE;
00188
00190
00192
00193 printf("----------------------------------------------------------------------\n");
00194 printf("Attempting to set communications mode to IMU Direct mode\n");
00195 printf("----------------------------------------------------------------------\n\n");
00196
00197 while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){}
00198
00200
00202
00203 while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK){}
00204
00205 if(com_mode == MIP_SDK_GX4_45_IMU_DIRECT_MODE)
00206 {
00207
00208 printf("Communications mode IMU Direct.\n");
00209
00210 printf("\n\n");
00211 Sleep(1500);
00212
00214
00215
00216
00218
00220
00222
00223 printf("----------------------------------------------------------------------\n");
00224 printf("Idling Device\n");
00225 printf("----------------------------------------------------------------------\n\n");
00226
00227 while(mip_base_cmd_idle(&device_interface) != MIP_INTERFACE_OK){}
00228
00229 printf("\n\n");
00230 Sleep(1500);
00231
00233
00235
00236 printf("----------------------------------------------------------------------\n");
00237 printf("Pinging Device\n");
00238 printf("----------------------------------------------------------------------\n\n");
00239
00240 while(mip_base_cmd_ping(&device_interface) != MIP_INTERFACE_OK){}
00241
00242 printf("\n\n");
00243 Sleep(1500);
00244
00246
00248
00249 printf("----------------------------------------------------------------------\n");
00250 printf("Getting Device Information\n");
00251 printf("----------------------------------------------------------------------\n\n");
00252
00253 while(mip_base_cmd_get_device_info(&device_interface, &device_info) != MIP_INTERFACE_OK){}
00254
00255 printf("\n\nDevice Info:\n");
00256 printf("---------------------------------------------\n");
00257
00258 memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00259 printf("Model Name => %s\n", temp_string);
00260
00261 memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00262 printf("Model Number => %s\n", temp_string);
00263
00264 memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00265 printf("Serial Number => %s\n", temp_string);
00266
00267 memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00268 printf("Lot Number => %s\n", temp_string);
00269
00270 memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00271 printf("Options => %s\n", temp_string);
00272
00273 printf("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000,
00274 (device_info.firmware_version)%1000/100,
00275 (device_info.firmware_version)%100);
00276
00277 printf("\n\n");
00278 Sleep(1500);
00279
00281
00283
00284 printf("----------------------------------------------------------------------\n");
00285 printf("Getting Supported descriptors\n");
00286 printf("----------------------------------------------------------------------\n\n");
00287
00288 while(mip_base_cmd_get_device_supported_descriptors(&device_interface, (u8*)device_descriptors, &device_descriptors_size) != MIP_INTERFACE_OK){}
00289
00290 printf("\n\nSupported descriptors:\n\n");
00291
00292 for(i=0; i< device_descriptors_size/2; i++)
00293 {
00294 printf("Descriptor Set: %02x, Descriptor: %02x\n", device_descriptors[i] >> 8, device_descriptors[i]&0xFF);
00295 Sleep(100);
00296 }
00297
00298 printf("\n\n");
00299 Sleep(1500);
00300
00302
00304
00305 printf("----------------------------------------------------------------------\n");
00306 printf("Running Built In Test\n");
00307 printf("----------------------------------------------------------------------\n\n");
00308
00309 while(mip_base_cmd_built_in_test(&device_interface, &bit_result) != MIP_INTERFACE_OK){}
00310
00311 printf("\nBIT Result (should be 0x00000000) => 0x%08x\n\n", bit_result);
00312
00313 printf("\n\n");
00314 Sleep(1500);
00315
00317
00318
00319
00321
00323
00325
00326 printf("----------------------------------------------------------------------\n");
00327 printf("Getting the AHRS datastream base rate\n");
00328 printf("----------------------------------------------------------------------\n\n");
00329
00330 while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00331
00332 printf("\nAHRS Base Rate => %d Hz\n\n", base_rate);
00333
00334 printf("\n\n");
00335 Sleep(1500);
00336
00338
00340
00341
00342 printf("----------------------------------------------------------------------\n");
00343 printf("Requesting BASIC Status Report:\n");
00344 printf("----------------------------------------------------------------------\n\n");
00345
00346
00347 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){}
00348
00349 printf("Model Number: \t\t\t\t\t%04u\n", imu_basic_field.device_model);
00350 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");
00351 printf("Status Flags: \t\t\t\t\t0x%08x\n", imu_basic_field.status_flags);
00352 printf("System Millisecond Timer Count: \t\t%llu ms\n\n", imu_basic_field.system_timer_ms);
00353
00354 printf("Requesting DIAGNOSTIC Status Report:\n");
00355
00356
00357 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){}
00358
00359 printf("Model Number: \t\t\t\t\t%04u\n", imu_diagnostic_field.device_model);
00360 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");
00361 printf("Status Flags: \t\t\t\t\t0x%08x\n", imu_diagnostic_field.status_flags);
00362 printf("System Millisecond Timer Count: \t\t%llu ms\n", imu_diagnostic_field.system_timer_ms);
00363 printf("Magnetometer: \t\t\t\t\t%s\n", imu_diagnostic_field.has_mag == 1 ? "DETECTED" : "NOT-DETECTED");
00364 printf("Pressure Sensor: \t\t\t\t%s\n", imu_diagnostic_field.has_pressure == 1 ? "DETECTED" : "NOT-DETECTED");
00365 printf("Gyro Range Reported: \t\t\t\t%u deg/s\n", imu_diagnostic_field.gyro_range);
00366 printf("Accel Range Reported: \t\t\t\t%u G\n", imu_diagnostic_field.accel_range);
00367 printf("Magnetometer Range Reported: \t\t\t%f Gs\n", imu_diagnostic_field.mag_range);
00368 printf("Pressure Range Reported: \t\t\t%f hPa\n", imu_diagnostic_field.pressure_range);
00369 printf("Measured Internal Temperature: \t\t\t%f degrees C\n", imu_diagnostic_field.temp_degc);
00370 printf("Last Temperature Measured: \t\t\t%u ms\n", imu_diagnostic_field.last_temp_read_ms);
00371 printf("Bad Temperature Sensor Detected: \t\t%s\n", imu_diagnostic_field.temp_sensor_error == 1 ? "TRUE" : "FALSE");
00372 printf("Number Received GPS Pulse-Per-Second Pulses: \t%u Pulses\n", imu_diagnostic_field.num_gps_pps_triggers);
00373 printf("Time of Last GPS Pulse-Per-Second Pulse: \t%u ms\n", imu_diagnostic_field.last_gps_pps_trigger_ms);
00374 printf("Data Streaming Enabled: \t\t\t%s\n", imu_diagnostic_field.stream_enabled == 1 ? "TRUE" : "FALSE");
00375 printf("Number of Dropped Communication Packets: \t%u packets\n", imu_diagnostic_field.dropped_packets);
00376 printf("Communications Port Bytes Written: \t\t%u Bytes\n", imu_diagnostic_field.com_port_bytes_written);
00377 printf("Communications Port Bytes Read: \t\t%u Bytes\n", imu_diagnostic_field.com_port_bytes_read);
00378 printf("Communications Port Write Overruns: \t\t%u Bytes\n", imu_diagnostic_field.com_port_write_overruns);
00379 printf("Communications Port Read Overruns: \t\t%u Bytes\n", imu_diagnostic_field.com_port_read_overruns);
00380
00381 printf("\n\n");
00382 Sleep(1500);
00383
00385
00387
00388 printf("----------------------------------------------------------------------\n");
00389 printf("Disabling Coning and Sculling compensation\n");
00390 printf("----------------------------------------------------------------------\n\n");
00391
00392 enable = MIP_3DM_CONING_AND_SCULLING_DISABLE;
00393
00394 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00395
00396 printf("Reading Coning and Sculling compensation enabled state:\n");
00397
00398 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00399
00400 printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00401
00402 printf("Enabling Coning and Sculling compensation.\n");
00403
00404 enable = MIP_3DM_CONING_AND_SCULLING_ENABLE;
00405
00406 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00407
00408 printf("Reading Coning and Sculling compensation enabled state:\n");
00409
00410 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00411
00412 printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00413
00414 printf("\n\n");
00415 Sleep(1500);
00416
00418
00420
00421 bias_vector[0] = 1.0f;
00422 bias_vector[1] = 2.0f;
00423 bias_vector[2] = 3.0f;
00424
00425 printf("----------------------------------------------------------------------\n");
00426 printf("Accel Bias Vector\n");
00427 printf("----------------------------------------------------------------------\n\n");
00428
00429 printf("Setting Accel Bias Vector:\n");
00430 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00431
00432 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00433
00434 memset(bias_vector, 0, 3*sizeof(float));
00435
00436 printf("Reading current Accel Bias Vector:\n");
00437
00438 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00439
00440 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00441
00442 printf("Resetting Accel Bias to default state.\n\n");
00443
00444 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00445
00446 printf("\n\n");
00447 Sleep(1500);
00448
00450
00452
00453 bias_vector[0] = 4.0f;
00454 bias_vector[1] = 5.0f;
00455 bias_vector[2] = 6.0f;
00456
00457 printf("----------------------------------------------------------------------\n");
00458 printf("Gyro Bias Vector\n");
00459 printf("----------------------------------------------------------------------\n\n");
00460
00461 printf("Setting Gyro Bias Vector:\n");
00462 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00463
00464 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00465
00466 memset(bias_vector, 0, 3*sizeof(float));
00467
00468 printf("Reading current Gyro Bias Vector:\n");
00469
00470 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00471
00472 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00473
00474 printf("Resetting Gyro Bias to default state.\n\n");
00475
00476 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00477
00478 printf("\n\n");
00479 Sleep(1500);
00480
00481
00483
00485
00486 printf("----------------------------------------------------------------------\n");
00487 printf("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n");
00488 printf("----------------------------------------------------------------------\n\n");
00489
00490 duration = 5000;
00491
00492 while(mip_3dm_cmd_capture_gyro_bias(&device_interface, duration, bias_vector) != MIP_INTERFACE_OK){}
00493
00494 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]);
00495
00496 printf("\n\n");
00497 Sleep(1500);
00498
00499
00501
00503
00504 printf("----------------------------------------------------------------------\n");
00505 printf("Setting the hard iron offset values\n");
00506 printf("----------------------------------------------------------------------\n\n");
00507
00508 hard_iron[0] = 1.0;
00509 hard_iron[1] = 2.0;
00510 hard_iron[2] = 3.0;
00511
00512 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, hard_iron) != MIP_INTERFACE_OK){}
00513
00514
00515 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, hard_iron_readback) != MIP_INTERFACE_OK){}
00516
00517 if((abs(hard_iron_readback[0] - hard_iron[0]) < 0.001) &&
00518 (abs(hard_iron_readback[1] - hard_iron[1]) < 0.001) &&
00519 (abs(hard_iron_readback[2] - hard_iron[2]) < 0.001))
00520 {
00521 printf("Hard iron offset values successfully set.\n");
00522 }
00523 else
00524 {
00525 printf("ERROR: Failed to set hard iron offset values!!!\n");
00526 printf("Sent values: %f X %f Y %f Z\n", hard_iron[0], hard_iron[1], hard_iron[2]);
00527 printf("Returned values: %f X %f Y %f Z\n", hard_iron_readback[0], hard_iron_readback[1], hard_iron_readback[2]);
00528 }
00529
00530 printf("\n\nLoading the default hard iron offset values.\n\n");
00531
00532 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00533
00534 printf("\n\n");
00535 Sleep(1500);
00536
00537
00539
00541
00542 printf("----------------------------------------------------------------------\n");
00543 printf("Setting the soft iron matrix values\n");
00544 printf("----------------------------------------------------------------------\n\n");
00545
00546 for(i=0; i<9; i++)
00547 soft_iron[i] = i;
00548
00549 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK){}
00550
00551
00552 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){}
00553
00554 if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) &&
00555 (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) &&
00556 (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) &&
00557 (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) &&
00558 (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) &&
00559 (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) &&
00560 (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) &&
00561 (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) &&
00562 (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001))
00563 {
00564 printf("Soft iron matrix values successfully set.\n");
00565 }
00566 else
00567 {
00568 printf("ERROR: Failed to set hard iron values!!!\n");
00569 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]);
00570 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],
00571 soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
00572 }
00573
00574 printf("\n\nLoading the default soft iron matrix values.\n\n");
00575
00576 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00577
00578 printf("\n\n");
00579 Sleep(1500);
00580
00581
00582
00584
00586
00587 printf("----------------------------------------------------------------------\n");
00588 printf("Setting the AHRS message format\n");
00589 printf("----------------------------------------------------------------------\n\n");
00590
00591 data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED;
00592 data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED;
00593
00594 data_stream_format_decimation[0] = 0x64;
00595 data_stream_format_decimation[1] = 0x64;
00596
00597 data_stream_format_num_entries = 2;
00598
00599 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){}
00600
00601 printf("\n\n");
00602 Sleep(1500);
00603
00604
00606
00608
00609 printf("----------------------------------------------------------------------\n");
00610 printf("Polling AHRS Data.\n");
00611 printf("----------------------------------------------------------------------\n\n");
00612
00613 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){}
00614
00615 }
00616 else
00617 {
00618 printf("ERROR: IMU_Direct mode not established\n\n");
00619 }
00620
00621 printf("\n\n");
00622 Sleep(1500);
00623
00624
00626
00627
00628
00630
00631 device_descriptors_size = 128*2;
00632 com_mode = MIP_SDK_GX4_45_IMU_STANDARD_MODE;
00633
00634 printf("----------------------------------------------------------------------\n");
00635 printf("Putting Device Into Standard Mode\n");
00636 printf("----------------------------------------------------------------------\n\n");
00637
00638
00639 while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK){}
00640
00641
00642 while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK){}
00643
00644 printf("\n\n");
00645 Sleep(1500);
00646
00647
00648 if(com_mode == MIP_SDK_GX4_45_IMU_STANDARD_MODE)
00649 {
00650
00652
00653
00654
00656
00658
00660
00661 printf("----------------------------------------------------------------------\n");
00662 printf("Idling Device\n");
00663 printf("----------------------------------------------------------------------\n\n");
00664
00665 while(mip_base_cmd_idle(&device_interface) != MIP_INTERFACE_OK){}
00666
00667 printf("\n\n");
00668 Sleep(1500);
00669
00671
00673
00674 printf("----------------------------------------------------------------------\n");
00675 printf("Pinging Device\n");
00676 printf("----------------------------------------------------------------------\n\n");
00677
00678 while(mip_base_cmd_ping(&device_interface) != MIP_INTERFACE_OK){}
00679
00680 printf("\n\n");
00681 Sleep(1500);
00682
00684
00686
00687 printf("----------------------------------------------------------------------\n");
00688 printf("Getting Device Information\n");
00689 printf("----------------------------------------------------------------------\n\n");
00690
00691 while(mip_base_cmd_get_device_info(&device_interface, &device_info) != MIP_INTERFACE_OK){}
00692
00693 printf("Device Info:\n");
00694 printf("---------------------------------------------\n");
00695
00696 memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00697 printf("Model Name => %s\n", temp_string);
00698
00699 memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00700 printf("Model Number => %s\n", temp_string);
00701
00702 memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00703 printf("Serial Number => %s\n", temp_string);
00704
00705 memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00706 printf("Lot Number => %s\n", temp_string);
00707
00708 memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2);
00709 printf("Options => %s\n", temp_string);
00710
00711 printf("Firmware Version => %d.%d.%.2d\n\n", (device_info.firmware_version)/1000,
00712 (device_info.firmware_version)%1000/100,
00713 (device_info.firmware_version)%100);
00714
00715 printf("\n\n");
00716 Sleep(1500);
00717
00719
00721
00722 printf("----------------------------------------------------------------------\n");
00723 printf("Getting Supported descriptors\n");
00724 printf("----------------------------------------------------------------------\n\n");
00725
00726 while(mip_base_cmd_get_device_supported_descriptors(&device_interface, (u8*)device_descriptors, &device_descriptors_size) != MIP_INTERFACE_OK){}
00727
00728 printf("\n\nSupported descriptors:\n\n");
00729
00730 for(i=0; i< device_descriptors_size/2; i++)
00731 {
00732 printf("Descriptor Set: %02x, Descriptor: %02x\n", device_descriptors[i] >> 8, device_descriptors[i]&0xFF);
00733 Sleep(100);
00734 }
00735
00736 printf("\n\n");
00737 Sleep(1500);
00738
00739
00741
00743
00744 printf("----------------------------------------------------------------------\n");
00745 printf("Running Built In Test\n");
00746 printf("----------------------------------------------------------------------\n\n");
00747
00748 while(mip_base_cmd_built_in_test(&device_interface, &bit_result) != MIP_INTERFACE_OK){}
00749
00750 printf("\nBIT Result (should be 0x00000000) => 0x%08x\n\n", bit_result);
00751
00752
00753 printf("\n\n");
00754 Sleep(1500);
00755
00757
00758
00759
00761
00763
00765
00766 printf("----------------------------------------------------------------------\n");
00767 printf("Getting the AHRS datastream base rate\n");
00768 printf("----------------------------------------------------------------------\n\n");
00769
00770 while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00771
00772 printf("\nAHRS Base Rate => %d Hz\n", base_rate);
00773
00774 printf("\n\n");
00775 Sleep(1500);
00776
00778
00780
00781 printf("----------------------------------------------------------------------\n");
00782 printf("Getting the GPS datastream base rate\n");
00783 printf("----------------------------------------------------------------------\n\n");
00784
00785 while(mip_3dm_cmd_get_gps_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00786
00787 printf("\nGPS Base Rate => %d Hz\n", base_rate);
00788
00789 printf("\n\n");
00790 Sleep(1500);
00791
00793
00795
00796 printf("----------------------------------------------------------------------\n");
00797 printf("Getting the Estimation Filter datastream base rate\n");
00798 printf("----------------------------------------------------------------------\n\n");
00799
00800 while(mip_3dm_cmd_get_filter_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK){}
00801
00802 printf("\nFILTER Base Rate => %d Hz\n", base_rate);
00803
00804 printf("\n\n");
00805 Sleep(1500);
00806
00808
00810
00811 printf("----------------------------------------------------------------------\n");
00812 printf("Toggling Coning and Sculling compensation\n");
00813 printf("----------------------------------------------------------------------\n\n");
00814
00815 enable = MIP_3DM_CONING_AND_SCULLING_DISABLE;
00816
00817 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00818
00819 printf("Reading Coning and Sculling compensation enabled state:\n");
00820
00821 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00822
00823 printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00824
00825 printf("Enabling Coning and Sculling compensation.\n");
00826
00827 enable = MIP_3DM_CONING_AND_SCULLING_ENABLE;
00828
00829 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &enable) != MIP_INTERFACE_OK){}
00830
00831 printf("Reading Coning and Sculling compensation enabled state:\n");
00832
00833 while(mip_3dm_cmd_coning_sculling_compensation(&device_interface, MIP_FUNCTION_SELECTOR_READ, &enable) != MIP_INTERFACE_OK){}
00834
00835 printf("%s\n\n", enable == MIP_3DM_CONING_AND_SCULLING_DISABLE ? "DISABLED" : "ENABLED");
00836
00837 printf("\n\n");
00838 Sleep(1500);
00839
00840
00842
00844
00845 bias_vector[0] = 1.0f;
00846 bias_vector[1] = 2.0f;
00847 bias_vector[2] = 3.0f;
00848
00849 printf("----------------------------------------------------------------------\n");
00850 printf("Accel Bias Vector\n");
00851 printf("----------------------------------------------------------------------\n\n");
00852
00853 printf("Setting Accel Bias Vector:\n");
00854 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00855
00856 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00857
00858 memset(bias_vector, 0, 3*sizeof(float));
00859
00860 printf("Reading current Accel Bias Vector:\n");
00861
00862 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00863
00864 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00865
00866 printf("Resetting Accel Bias to default state.\n\n");
00867
00868 while(mip_3dm_cmd_accel_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00869
00870 printf("\n\n");
00871 Sleep(1500);
00872
00873
00875
00877
00878 bias_vector[0] = 4.0f;
00879 bias_vector[1] = 5.0f;
00880 bias_vector[2] = 6.0f;
00881
00882 printf("----------------------------------------------------------------------\n");
00883 printf("Gyro Bias Vector\n");
00884 printf("----------------------------------------------------------------------\n\n");
00885
00886 printf("Setting Gyro Bias Vector:\n");
00887 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00888
00889 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, bias_vector) != MIP_INTERFACE_OK){}
00890
00891 memset(bias_vector, 0, 3*sizeof(float));
00892
00893 printf("Reading current Gyro Bias Vector:\n");
00894
00895 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_READ, bias_vector) != MIP_INTERFACE_OK){}
00896
00897 printf("bias_vector[0] = %f\nbias_vector[1] = %f\nbias_vector[2] = %f\n\n", bias_vector[0], bias_vector[1], bias_vector[2]);
00898
00899 printf("Resetting Gyro Bias to default state.\n\n");
00900
00901 while(mip_3dm_cmd_gyro_bias(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00902
00903 printf("\n\n");
00904 Sleep(1500);
00905
00906
00908
00910
00911 printf("----------------------------------------------------------------------\n");
00912 printf("Performing Gyro Bias capture.\nPlease keep device stationary during the 5 second gyro bias capture interval\n");
00913 printf("----------------------------------------------------------------------\n\n");
00914
00915 duration = 5000;
00916
00917 while(mip_3dm_cmd_capture_gyro_bias(&device_interface, duration, bias_vector) != MIP_INTERFACE_OK){}
00918
00919 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]);
00920
00921 printf("\n\n");
00922 Sleep(1500);
00923
00924
00926
00928
00929 printf("----------------------------------------------------------------------\n");
00930 printf("Setting the hard iron offset values\n");
00931 printf("----------------------------------------------------------------------\n\n");
00932
00933 hard_iron[0] = 1.0;
00934 hard_iron[1] = 2.0;
00935 hard_iron[2] = 3.0;
00936
00937 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, hard_iron) != MIP_INTERFACE_OK){}
00938
00939
00940 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, hard_iron_readback) != MIP_INTERFACE_OK){}
00941
00942 if((abs(hard_iron_readback[0] - hard_iron[0]) < 0.001) &&
00943 (abs(hard_iron_readback[1] - hard_iron[1]) < 0.001) &&
00944 (abs(hard_iron_readback[2] - hard_iron[2]) < 0.001))
00945 {
00946 printf("Hard iron offset values successfully set.\n");
00947 }
00948 else
00949 {
00950 printf("ERROR: Failed to set hard iron offset values!!!\n");
00951 printf("Sent values: %f X %f Y %f Z\n", hard_iron[0], hard_iron[1], hard_iron[2]);
00952 printf("Returned values: %f X %f Y %f Z\n", hard_iron_readback[0], hard_iron_readback[1], hard_iron_readback[2]);
00953 }
00954
00955 printf("\n\nLoading the default hard iron offset values.\n\n");
00956
00957 while(mip_3dm_cmd_hard_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
00958
00959 printf("\n\n");
00960 Sleep(1500);
00961
00962
00964
00966
00967 printf("----------------------------------------------------------------------\n");
00968 printf("Setting the soft iron matrix values\n");
00969 printf("----------------------------------------------------------------------\n\n");
00970
00971 for(i=0; i<9; i++)
00972 soft_iron[i] = i;
00973
00974 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, soft_iron) != MIP_INTERFACE_OK){}
00975
00976
00977 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_READ, soft_iron_readback) != MIP_INTERFACE_OK){}
00978
00979 if((abs(soft_iron_readback[0] - soft_iron[0]) < 0.001) &&
00980 (abs(soft_iron_readback[1] - soft_iron[1]) < 0.001) &&
00981 (abs(soft_iron_readback[2] - soft_iron[2]) < 0.001) &&
00982 (abs(soft_iron_readback[3] - soft_iron[3]) < 0.001) &&
00983 (abs(soft_iron_readback[4] - soft_iron[4]) < 0.001) &&
00984 (abs(soft_iron_readback[5] - soft_iron[5]) < 0.001) &&
00985 (abs(soft_iron_readback[6] - soft_iron[6]) < 0.001) &&
00986 (abs(soft_iron_readback[7] - soft_iron[7]) < 0.001) &&
00987 (abs(soft_iron_readback[8] - soft_iron[8]) < 0.001))
00988 {
00989 printf("Soft iron matrix values successfully set.\n");
00990 }
00991 else
00992 {
00993 printf("ERROR: Failed to set hard iron values!!!\n");
00994 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]);
00995 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],
00996 soft_iron_readback[5], soft_iron_readback[6], soft_iron_readback[7], soft_iron_readback[8]);
00997 }
00998
00999 printf("\n\nLoading the default soft iron matrix values.\n\n");
01000
01001 while(mip_3dm_cmd_soft_iron(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01002
01003 printf("\n\n");
01004 Sleep(1500);
01005
01006
01007
01009
01011
01012 printf("----------------------------------------------------------------------\n");
01013 printf("Setting the complementary filter values\n");
01014 printf("----------------------------------------------------------------------\n\n");
01015
01016
01017 comp_filter_command.north_compensation_enable = 0;
01018 comp_filter_command.up_compensation_enable = 0;
01019
01020 comp_filter_command.north_compensation_time_constant = 30;
01021 comp_filter_command.up_compensation_time_constant = 30;
01022
01023 while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &comp_filter_command) != MIP_INTERFACE_OK){}
01024
01025
01026 while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_READ, &comp_filter_readback) != MIP_INTERFACE_OK){}
01027
01028 if((comp_filter_command.north_compensation_enable == comp_filter_readback.north_compensation_enable) &&
01029 (comp_filter_command.up_compensation_enable == comp_filter_readback.up_compensation_enable) &&
01030 (abs(comp_filter_command.north_compensation_time_constant - comp_filter_readback.north_compensation_time_constant) < 0.001) &&
01031 (abs(comp_filter_command.up_compensation_time_constant - comp_filter_readback.up_compensation_time_constant) < 0.001))
01032 {
01033 printf("Complementary filter values successfully set.\n");
01034 }
01035 else
01036 {
01037 printf("ERROR: Failed to set complementary filter values!!!\n");
01038 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,
01039 comp_filter_command.up_compensation_time_constant, comp_filter_command.north_compensation_time_constant);
01040 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,
01041 comp_filter_readback.up_compensation_time_constant, comp_filter_readback.north_compensation_time_constant);
01042 }
01043
01044 printf("\n\nLoading the default complementary filter values.\n\n");
01045
01046 while(mip_3dm_cmd_complementary_filter_settings(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01047
01048 printf("\n\n");
01049 Sleep(1500);
01050
01051
01053
01055
01056 printf("----------------------------------------------------------------------\n");
01057 printf("Requesting BASIC Status Report\n");
01058 printf("----------------------------------------------------------------------\n\n");
01059
01060
01061 while(mip_3dm_cmd_hw_specific_device_status(&device_interface, GX4_45_MODEL_NUMBER, GX4_45_BASIC_STATUS_SEL, &basic_field) != MIP_INTERFACE_OK){}
01062
01063 printf("Model Number: \t\t\t\t\t%04u\n", basic_field.device_model);
01064 printf("Status Selector: \t\t\t\t%s\n", basic_field.status_selector == GX4_45_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
01065 printf("Status Flags: \t\t\t\t\t0x%08x\n", basic_field.status_flags);
01066
01067 if(basic_field.system_state == GX4_45_SYSTEM_STATE_INIT)
01068 {
01069 strcpy(temp_string,"System Initialization");
01070 }
01071 else if(basic_field.system_state == GX4_45_SYSTEM_STATE_SENSOR_STARTUP)
01072 {
01073 strcpy(temp_string,"Sensor Start-up");
01074 }
01075 else if(basic_field.system_state == GX4_45_SYSTEM_STATE_RUNNING)
01076 {
01077 strcpy(temp_string,"System Running");
01078 }
01079
01080 printf("System State: \t\t\t\t\t%s\n",temp_string);
01081 printf("System Microsecond Timer Count: \t\t%llu ms\n\n", basic_field.system_timer_ms);
01082
01083 printf("Requesting DIAGNOSTIC Status Report:\n");
01084
01085
01086 while(mip_3dm_cmd_hw_specific_device_status(&device_interface, GX4_45_MODEL_NUMBER, GX4_45_DIAGNOSTICS_STATUS_SEL, &diagnostic_field) != MIP_INTERFACE_OK){}
01087
01088 printf("Model Number: \t\t\t\t\t%04u\n", diagnostic_field.device_model);
01089 printf("Status Selector: \t\t\t\t%s\n", diagnostic_field.status_selector == GX4_45_BASIC_STATUS_SEL ? "Basic Status Report" : "Diagnostic Status Report");
01090 printf("Status Flags: \t\t\t\t\t0x%08x\n", diagnostic_field.status_flags);
01091 printf("System Millisecond Timer Count: \t\t%llu ms\n", diagnostic_field.system_timer_ms);
01092 printf("Number Received GPS Pulse-Per-Second Pulses: \t%u Pulses\n", diagnostic_field.num_gps_pps_triggers);
01093 printf("Time of Last GPS Pulse-Per-Second Pulse: \t%u ms\n", diagnostic_field.last_gps_pps_trigger_ms);
01094 printf("IMU Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.imu_stream_enabled == 1 ? "TRUE" : "FALSE");
01095 printf("GPS Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.gps_stream_enabled == 1 ? "TRUE" : "FALSE");
01096 printf("FILTER Streaming Enabled: \t\t\t\t%s\n", diagnostic_field.filter_stream_enabled == 1 ? "TRUE" : "FALSE");
01097 printf("Number of Dropped IMU Packets: \t\t\t%u packets\n", diagnostic_field.imu_dropped_packets);
01098 printf("Number of Dropped GPS Packets: \t\t\t%u packets\n", diagnostic_field.gps_dropped_packets);
01099 printf("Number of Dropped FILTER Packets: \t\t\t%u packets\n", diagnostic_field.filter_dropped_packets);
01100 printf("Communications Port Bytes Written: \t\t%u Bytes\n", diagnostic_field.com1_port_bytes_written);
01101 printf("Communications Port Bytes Read: \t\t%u Bytes\n", diagnostic_field.com1_port_bytes_read);
01102 printf("Communications Port Write Overruns: \t\t%u Bytes\n", diagnostic_field.com1_port_write_overruns);
01103 printf("Communications Port Read Overruns: \t\t%u Bytes\n", diagnostic_field.com1_port_read_overruns);
01104 printf("IMU Parser Errors: \t\t\t\t%u Errors\n", diagnostic_field.imu_parser_errors);
01105 printf("IMU Message Count: \t\t\t\t%u Messages\n", diagnostic_field.imu_message_count);
01106 printf("IMU Last Message Received: \t\t\t%u ms\n", diagnostic_field.imu_last_message_ms);
01107 printf("GPS Parser Errors: \t\t\t\t%u Errors\n", diagnostic_field.gps_parser_errors);
01108 printf("GPS Message Count: \t\t\t\t%u Messages\n", diagnostic_field.gps_message_count);
01109 printf("GPS Last Message Received: \t\t\t%u ms\n", diagnostic_field.gps_last_message_ms);
01110
01111 printf("\n\n");
01112 Sleep(1500);
01113
01115
01116
01117
01119
01120
01122
01124
01125 printf("----------------------------------------------------------------------\n");
01126 printf("Resetting the Filter\n");
01127 printf("----------------------------------------------------------------------\n\n");
01128
01129 while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01130
01131 printf("\n\n");
01132 Sleep(1500);
01133
01135
01137
01138 printf("----------------------------------------------------------------------\n");
01139 printf("Initializing the Filter with Euler angles\n");
01140 printf("----------------------------------------------------------------------\n\n");
01141
01142 angles[0] = angles[1] = angles[2] = 0;
01143
01144 while(mip_filter_set_init_attitude(&device_interface, angles) != MIP_INTERFACE_OK){}
01145
01146 printf("\n\n");
01147 Sleep(1500);
01148
01150
01152
01153 printf("----------------------------------------------------------------------\n");
01154 printf("Resetting the Filter\n");
01155 printf("----------------------------------------------------------------------\n\n");
01156
01157 while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01158
01159 printf("\n\n");
01160 Sleep(1500);
01161
01163
01165
01166 printf("----------------------------------------------------------------------\n");
01167 printf("Initializing the Filter with a heading angle\n");
01168 printf("----------------------------------------------------------------------\n\n");
01169
01170 while(mip_filter_set_init_heading(&device_interface, angles[0]) != MIP_INTERFACE_OK){}
01171
01172 printf("\n\n");
01173 Sleep(1500);
01174
01176
01178
01179 printf("----------------------------------------------------------------------\n");
01180 printf("Resetting the Filter\n");
01181 printf("----------------------------------------------------------------------\n\n");
01182
01183 while(mip_filter_reset_filter(&device_interface) != MIP_INTERFACE_OK){}
01184
01185 printf("\n\n");
01186 Sleep(1500);
01187
01189
01191
01192 printf("----------------------------------------------------------------------\n");
01193 printf("Cycle through available Vehicle Dynamics Modes\n");
01194 printf("----------------------------------------------------------------------\n\n");
01195
01196
01197 for(i=3; i>=1; i--)
01198 {
01199
01200 dynamics_mode = i;
01201
01202 while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &dynamics_mode) != MIP_INTERFACE_OK){}
01203
01204
01205 readback_dynamics_mode = 0;
01206
01207
01208 while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_dynamics_mode) != MIP_INTERFACE_OK){}
01209
01210 if(dynamics_mode == readback_dynamics_mode)
01211 {
01212 printf("Vehicle dynamics mode successfully set to %d\n", dynamics_mode);
01213 }
01214 else
01215 {
01216 printf("ERROR: Failed to set vehicle dynamics mode to %d!!!\n", dynamics_mode);
01217 }
01218 }
01219
01220 printf("\nLoading the default vehicle dynamics mode\n\n");
01221
01222 while(mip_filter_vehicle_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01223
01224 printf("\n\n");
01225 Sleep(1500);
01226
01227
01229
01231
01232 printf("----------------------------------------------------------------------\n");
01233 printf("Setting the sensor to vehicle frame transformation\n");
01234 printf("----------------------------------------------------------------------\n\n");
01235
01236 angles[0] = 3.14/4.0;
01237 angles[1] = 3.14/8.0;
01238 angles[2] = 3.14/12.0;
01239
01240 while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, angles) != MIP_INTERFACE_OK){}
01241
01242
01243 while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_angles) != MIP_INTERFACE_OK){}
01244
01245 if((abs(readback_angles[0]-angles[0]) < 0.001) &&
01246 (abs(readback_angles[1]-angles[1]) < 0.001) &&
01247 (abs(readback_angles[2]-angles[2]) < 0.001))
01248 {
01249 printf("Transformation successfully set.\n");
01250 }
01251 else
01252 {
01253 printf("ERROR: Failed to set transformation!!!\n");
01254 printf("Sent angles: %f roll %f pitch %f yaw\n", angles[0], angles[1], angles[2]);
01255 printf("Returned angles: %f roll %f pitch %f yaw\n", readback_angles[0], readback_angles[1], readback_angles[2]);
01256 }
01257
01258 printf("\n\nLoading the default sensor to vehicle transformation.\n\n");
01259
01260 while(mip_filter_sensor2vehicle_tranformation(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01261
01262 printf("\n\n");
01263 Sleep(1500);
01264
01266
01268
01269 printf("----------------------------------------------------------------------\n");
01270 printf("Setting the sensor to vehicle frame offset\n");
01271 printf("----------------------------------------------------------------------\n\n");
01272
01273 offset[0] = 1.0;
01274 offset[1] = 2.0;
01275 offset[2] = 3.0;
01276
01277 while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK){}
01278
01279
01280 while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){}
01281
01282 if((abs(readback_offset[0]-offset[0]) < 0.001) &&
01283 (abs(readback_offset[1]-offset[1]) < 0.001) &&
01284 (abs(readback_offset[2]-offset[2]) < 0.001))
01285 {
01286 printf("Offset successfully set.\n");
01287 }
01288 else
01289 {
01290 printf("ERROR: Failed to set offset!!!\n");
01291 printf("Sent offset: %f X %f Y %f Z\n", offset[0], offset[1], offset[2]);
01292 printf("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
01293 }
01294
01295 printf("\n\nLoading the default sensor to vehicle offset.\n\n");
01296
01297 while(mip_filter_sensor2vehicle_offset(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01298
01299 printf("\n\n");
01300 Sleep(1500);
01301
01303
01305
01306 printf("----------------------------------------------------------------------\n");
01307 printf("Setting the GPS antenna offset\n");
01308 printf("----------------------------------------------------------------------\n\n");
01309
01310 offset[0] = 1.0;
01311 offset[1] = 2.0;
01312 offset[2] = 3.0;
01313
01314 while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, offset) != MIP_INTERFACE_OK){}
01315
01316
01317 while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_offset) != MIP_INTERFACE_OK){}
01318
01319 if((abs(readback_offset[0]-offset[0]) < 0.001) &&
01320 (abs(readback_offset[1]-offset[1]) < 0.001) &&
01321 (abs(readback_offset[2]-offset[2]) < 0.001))
01322 {
01323 printf("Offset successfully set.\n");
01324 }
01325 else
01326 {
01327 printf("ERROR: Failed to set offset!!!\n");
01328 printf("Sent offset: %f X %f Y %f Z\n", offset[0], offset[1], offset[2]);
01329 printf("Returned offset: %f X %f Y %f Z\n", readback_offset[0], readback_offset[1], readback_offset[2]);
01330 }
01331
01332 printf("\n\nLoading the default antenna offset.\n\n");
01333
01334 while(mip_filter_antenna_offset(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01335
01336 printf("\n\n");
01337 Sleep(1500);
01338
01340
01342
01343 printf("----------------------------------------------------------------------\n");
01344 printf("Cycling through Estimation Control Flags\n");
01345 printf("----------------------------------------------------------------------\n\n");
01346
01347
01348 for(j=0; j < 0x0020 ; j++)
01349 {
01350
01351 estimation_control = (0xFFFF & ~j);
01352
01353
01354 while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &estimation_control) != MIP_INTERFACE_OK){}
01355
01356
01357 while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &estimation_control_readback) != MIP_INTERFACE_OK){}
01358
01359 if(estimation_control != estimation_control_readback)
01360 {
01361 printf("ERROR:\n");
01362
01363 if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GYRO_BIAS) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GYRO_BIAS) == 0)
01364 printf("Gyroscope Bias Estimation NOT DISABLED\n");
01365
01366 if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_ACCEL_BIAS) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_ACCEL_BIAS) == 0)
01367 printf("Accelerometer Bias Estimation NOT DISABLED\n");
01368
01369 if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GYRO_SCALE_FACTOR) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GYRO_SCALE_FACTOR) == 0)
01370 printf("Gyroscope Scale Factor Estimation NOT DISABLED\n");
01371
01372 if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_ACCEL_SCALE_FACTOR) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_ACCEL_SCALE_FACTOR) == 0)
01373 printf("Accelerometer Scale Factor Estimation NOT DISABLED\n");
01374
01375 if((estimation_control_readback & FILTER_ESTIMATION_CONTROL_GPS_ANTENNA_OFFSET) != 0 && (estimation_control & FILTER_ESTIMATION_CONTROL_GPS_ANTENNA_OFFSET) == 0)
01376 printf("GPS Antenna Offset Estimation NOT DISABLED\n");
01377 }
01378 }
01379
01380 printf("\n\nResetting Estimation Control Flags to default state.\n\n");
01381
01382 while(mip_filter_estimation_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, &estimation_control) != MIP_INTERFACE_OK){}
01383
01384 printf("\n\n");
01385 Sleep(1500);
01386
01388
01390
01391 printf("----------------------------------------------------------------------\n");
01392 printf("Cycle through available GPS sources\n");
01393 printf("----------------------------------------------------------------------\n\n");
01394
01395
01396 for(i=2; i>=1; i--)
01397 {
01398
01399 gps_source = i;
01400
01401 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01402
01403
01404 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &gps_source) != MIP_INTERFACE_OK){}
01405
01406 if(gps_source == i)
01407 {
01408 printf("GPS source successfully set to %d\n", i);
01409 }
01410 else
01411 {
01412 printf("ERROR: Failed to set GPS source to %d!!!\n", i);
01413 }
01414 }
01415
01416 printf("\n\nLoading the default gps source.\n\n");
01417
01418 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01419
01420 printf("\n\n");
01421 Sleep(1500);
01422
01424
01426
01427
01428 printf("----------------------------------------------------------------------\n");
01429 printf("Performing External GPS Update with externally supplied GPS information\n");
01430 printf("----------------------------------------------------------------------\n\n");
01431
01432 gps_source = 0x2;
01433
01434 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01435
01436
01437 external_gps_update.tow = 0;
01438 external_gps_update.week_number = 0;
01439 external_gps_update.pos[0] = 44.43753433;
01440 external_gps_update.pos[1] = -73.10612488;
01441 external_gps_update.pos[2] = 134.029999;
01442 external_gps_update.vel[0] = 0.0;
01443 external_gps_update.vel[1] = 0.0;
01444 external_gps_update.vel[2] = 0.0;
01445 external_gps_update.pos_1sigma[0] = 0.1;
01446 external_gps_update.pos_1sigma[1] = 0.1;
01447 external_gps_update.pos_1sigma[2] = 0.1;
01448 external_gps_update.vel_1sigma[0] = 0.1;
01449 external_gps_update.vel_1sigma[1] = 0.1;
01450 external_gps_update.vel_1sigma[2] = 0.1;
01451
01452 while(mip_filter_external_gps_update(&device_interface, &external_gps_update) != MIP_INTERFACE_OK){}
01453
01454
01455 gps_source = 0x01;
01456
01457 while(mip_filter_gps_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_source) != MIP_INTERFACE_OK){}
01458
01459 printf("\n\n");
01460 Sleep(1500);
01461
01463
01465
01466 printf("----------------------------------------------------------------------\n");
01467 printf("Performing External Heading Update with externally supplied heading information\n");
01468 printf("----------------------------------------------------------------------\n\n");
01469
01470
01471 heading_source = 0x3;
01472
01473 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK){}
01474
01475 external_heading_update.heading_angle = 0.0;
01476 external_heading_update.heading_angle_1sigma = 0.01;
01477 external_heading_update.type = 0x1;
01478
01479 while(mip_filter_external_heading_update(&device_interface, &external_heading_update) != MIP_INTERFACE_OK){}
01480
01481
01482
01483 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, &heading_source) != MIP_INTERFACE_OK){}
01484
01485 printf("\n\n");
01486 Sleep(1500);
01487
01489
01491
01492 printf("----------------------------------------------------------------------\n");
01493 printf("Cycle through available Heading sources\n");
01494 printf("----------------------------------------------------------------------\n\n");
01495
01496
01497 for(i=3; i>=0; i--)
01498 {
01499
01500 heading_source = i;
01501
01502 mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source);
01503
01504
01505 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &heading_source) != MIP_INTERFACE_OK){}
01506
01507 if(heading_source == i)
01508 {
01509 printf("Heading source successfully set to %d\n", i);
01510 }
01511 else
01512 {
01513 printf("ERROR: Failed to set heading source to %d!!!\n", i);
01514 }
01515 }
01516
01517 printf("\n\nLoading the default heading source.\n\n");
01518
01519 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01520
01521 printf("\n\n");
01522 Sleep(1500);
01523
01525
01527
01528 printf("----------------------------------------------------------------------\n");
01529 printf("Cycle through available auto-init values\n");
01530 printf("----------------------------------------------------------------------\n\n");
01531
01532
01533 for(i=1; i>=0; i--)
01534 {
01535
01536 auto_init = i;
01537
01538 while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &auto_init) != MIP_INTERFACE_OK){}
01539
01540
01541 while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_READ, &auto_init) != MIP_INTERFACE_OK){}
01542
01543 if(auto_init == i)
01544 {
01545 printf("Auto-init successfully set to %d\n", i);
01546 }
01547 else
01548 {
01549 printf("ERROR: Failed to set auto-init to %d!!!\n", i);
01550 }
01551 }
01552
01553 printf("\n\nLoading the default auto-init value.\n\n");
01554
01555 while(mip_filter_auto_initialization(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01556
01557 printf("\n\n");
01558 Sleep(1500);
01559
01561
01563
01564 printf("----------------------------------------------------------------------\n");
01565 printf("Setting the accel noise values\n");
01566 printf("----------------------------------------------------------------------\n\n");
01567
01568 noise[0] = 0.1;
01569 noise[1] = 0.2;
01570 noise[2] = 0.3;
01571
01572 while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01573
01574
01575 while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01576
01577 if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01578 (abs(readback_noise[1]-noise[1]) < 0.001) &&
01579 (abs(readback_noise[2]-noise[2]) < 0.001))
01580 {
01581 printf("Accel noise values successfully set.\n");
01582 }
01583 else
01584 {
01585 printf("ERROR: Failed to set accel noise values!!!\n");
01586 printf("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01587 printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01588 }
01589
01590 printf("\n\nLoading the default accel noise values.\n\n");
01591
01592 while(mip_filter_accel_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01593
01594 printf("\n\n");
01595 Sleep(1500);
01596
01598
01600
01601 printf("----------------------------------------------------------------------\n");
01602 printf("Setting the gyro noise values\n");
01603 printf("----------------------------------------------------------------------\n\n");
01604
01605 noise[0] = 0.1;
01606 noise[1] = 0.2;
01607 noise[2] = 0.3;
01608
01609 while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01610
01611
01612 while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01613
01614 if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01615 (abs(readback_noise[1]-noise[1]) < 0.001) &&
01616 (abs(readback_noise[2]-noise[2]) < 0.001))
01617 {
01618 printf("Gyro noise values successfully set.\n");
01619 }
01620 else
01621 {
01622 printf("ERROR: Failed to set gyro noise values!!!\n");
01623 printf("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01624 printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01625 }
01626
01627 printf("\n\nLoading the default gyro noise values.\n\n");
01628
01629 while(mip_filter_gyro_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01630
01631 printf("\n\n");
01632 Sleep(1500);
01633
01634
01636
01638
01639 printf("----------------------------------------------------------------------\n");
01640 printf("Setting the mag noise values\n");
01641 printf("----------------------------------------------------------------------\n\n");
01642
01643 noise[0] = 0.1;
01644 noise[1] = 0.2;
01645 noise[2] = 0.3;
01646
01647 while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, noise) != MIP_INTERFACE_OK){}
01648
01649
01650 while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_noise) != MIP_INTERFACE_OK){}
01651
01652 if((abs(readback_noise[0] - noise[0]) < 0.001) &&
01653 (abs(readback_noise[1] - noise[1]) < 0.001) &&
01654 (abs(readback_noise[2] - noise[2]) < 0.001))
01655 {
01656 printf("Mag noise values successfully set.\n");
01657 }
01658 else
01659 {
01660 printf("ERROR: Failed to set mag noise values!!!\n");
01661 printf("Sent values: %f X %f Y %f Z\n", noise[0], noise[1], noise[2]);
01662 printf("Returned values: %f X %f Y %f Z\n", readback_noise[0], readback_noise[1], readback_noise[2]);
01663 }
01664
01665 printf("\n\nLoading the default mag noise values.\n\n");
01666
01667 while(mip_filter_mag_noise(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01668
01669 printf("\n\n");
01670 Sleep(1500);
01671
01673
01675
01676 printf("----------------------------------------------------------------------\n");
01677 printf("Setting the accel bias model values\n");
01678 printf("----------------------------------------------------------------------\n\n");
01679
01680 noise[0] = 0.1;
01681 noise[1] = 0.2;
01682 noise[2] = 0.3;
01683
01684 beta[0] = 0.1;
01685 beta[1] = 0.2;
01686 beta[2] = 0.3;
01687
01688 while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){}
01689
01690
01691 while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){}
01692
01693 if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01694 (abs(readback_noise[1]-noise[1]) < 0.001) &&
01695 (abs(readback_noise[2]-noise[2]) < 0.001) &&
01696 (abs(readback_beta[0]-beta[0]) < 0.001) &&
01697 (abs(readback_beta[1]-beta[1]) < 0.001) &&
01698 (abs(readback_beta[2]-beta[2]) < 0.001))
01699 {
01700 printf("Accel bias model values successfully set.\n");
01701 }
01702 else
01703 {
01704 printf("ERROR: Failed to set accel bias model values!!!\n");
01705 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]);
01706 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],
01707 readback_noise[0], readback_noise[1], readback_noise[2]);
01708 }
01709
01710 printf("\n\nLoading the default accel bias model values.\n\n");
01711
01712 while(mip_filter_accel_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL, NULL) != MIP_INTERFACE_OK){}
01713
01714 printf("\n\n");
01715 Sleep(1500);
01716
01718
01720
01721 printf("----------------------------------------------------------------------\n");
01722 printf("Setting the gyro bias model values\n");
01723 printf("----------------------------------------------------------------------\n\n");
01724
01725 noise[0] = 0.1;
01726 noise[1] = 0.2;
01727 noise[2] = 0.3;
01728
01729 beta[0] = 0.1;
01730 beta[1] = 0.2;
01731 beta[2] = 0.3;
01732
01733 while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, beta, noise) != MIP_INTERFACE_OK){}
01734
01735
01736 while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_READ, readback_beta, readback_noise) != MIP_INTERFACE_OK){}
01737
01738 if((abs(readback_noise[0]-noise[0]) < 0.001) &&
01739 (abs(readback_noise[1]-noise[1]) < 0.001) &&
01740 (abs(readback_noise[2]-noise[2]) < 0.001) &&
01741 (abs(readback_beta[0]-beta[0]) < 0.001) &&
01742 (abs(readback_beta[1]-beta[1]) < 0.001) &&
01743 (abs(readback_beta[2]-beta[2]) < 0.001))
01744 {
01745 printf("Gyro bias model values successfully set.\n");
01746 }
01747 else
01748 {
01749 printf("ERROR: Failed to set gyro bias model values!!!\n");
01750 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]);
01751 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],
01752 readback_noise[0], readback_noise[1], readback_noise[2]);
01753 }
01754
01755 printf("\n\nLoading the default gyro bias model values.\n\n");
01756
01757 while(mip_filter_gyro_bias_model(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL, NULL) != MIP_INTERFACE_OK){}
01758
01759 printf("\n\n");
01760 Sleep(1500);
01761
01763
01765
01766 printf("----------------------------------------------------------------------\n");
01767 printf("Setting Zero Velocity-Update threshold\n");
01768 printf("----------------------------------------------------------------------\n\n");
01769
01770 zero_update_control.threshold = 0.1;
01771 zero_update_control.enable = 1;
01772
01773
01774 while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01775
01776
01777 while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){}
01778
01779 if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold)
01780 printf("ERROR configuring Zero Velocity Update.\n");
01781
01782 printf("\n\nResetting Zero Velocity Update Control to default parameters.\n\n");
01783
01784
01785 while(mip_filter_zero_velocity_update_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01786
01787 printf("\n\n");
01788 Sleep(1500);
01789
01791
01793
01794 printf("----------------------------------------------------------------------\n");
01795 printf("Applying External Heading Update with Timestamp\n");
01796 printf("----------------------------------------------------------------------\n\n");
01797
01798
01799 heading_source = 0x3;
01800
01801 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &heading_source) != MIP_INTERFACE_OK){}
01802
01803 external_heading_with_time.gps_tow = 0.0;
01804 external_heading_with_time.gps_week_number = 0;
01805 external_heading_with_time.heading_angle_rads = 0.0;
01806 external_heading_with_time.heading_angle_sigma_rads = 0.05;
01807 external_heading_with_time.heading_type = 0x01;
01808
01809 while(mip_filter_external_heading_update_with_time(&device_interface, &external_heading_with_time) != MIP_INTERFACE_OK){}
01810
01811
01812 printf("\n\nResetting default heading update.\n\n");
01813
01814 while(mip_filter_heading_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01815
01816 printf("\n\n");
01817 Sleep(1500);
01818
01820
01822
01823 printf("----------------------------------------------------------------------\n");
01824 printf("Setting Zero Angular-Rate-Update threshold\n");
01825 printf("----------------------------------------------------------------------\n\n");
01826
01827 zero_update_control.threshold = 0.05;
01828 zero_update_control.enable = 1;
01829
01830
01831 while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &zero_update_control) != MIP_INTERFACE_OK){}
01832
01833
01834 while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_READ, &zero_update_readback) != MIP_INTERFACE_OK){}
01835
01836 if(zero_update_control.enable != zero_update_readback.enable || zero_update_control.threshold != zero_update_readback.threshold)
01837 printf("ERROR configuring Zero Angular Rate Update.\n");
01838
01839 printf("\n\nResetting Zero Angular Rate Update Control to default parameters.\n\n");
01840
01841
01842 while(mip_filter_zero_angular_rate_update_control(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01843
01844
01845 printf("\n\n");
01846 Sleep(1500);
01847
01848
01850
01852
01853 printf("----------------------------------------------------------------------\n");
01854 printf("Performing Tare Orientation Command\n");
01855 printf("(This will only pass if the internal GPS solution is valid)\n");
01856 printf("----------------------------------------------------------------------\n\n");
01857
01858
01859 printf("Re-initializing filter (required for tare)\n\n");
01860
01861
01862 angles[0] = angles[1] = angles[2] = 0;
01863
01864 while(mip_filter_set_init_attitude(&device_interface, angles) != MIP_INTERFACE_OK){}
01865
01866
01867 Sleep(5000);
01868
01869
01870
01871 for(i=1; i<8; i++)
01872 {
01873
01874 if(mip_filter_tare_orientation(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, i) != MIP_INTERFACE_OK)
01875 {
01876 printf("ERROR: Failed Axis - ");
01877
01878 if(i & FILTER_TARE_ROLL_AXIS)
01879 printf(" Roll Axis ");
01880
01881 if(i & FILTER_TARE_PITCH_AXIS)
01882 printf(" Pitch Axis ");
01883
01884 if(i & FILTER_TARE_YAW_AXIS)
01885 printf(" Yaw Axis ");
01886 }
01887 else
01888 {
01889 printf("Tare Configuration = %d\n", i);
01890
01891 printf("Tared -");
01892
01893 if(i & FILTER_TARE_ROLL_AXIS)
01894 printf(" Roll Axis ");
01895
01896 if(i & FILTER_TARE_PITCH_AXIS)
01897 printf(" Pitch Axis ");
01898
01899 if(i & FILTER_TARE_YAW_AXIS)
01900 printf(" Yaw Axis ");
01901
01902 printf("\n\n");
01903 }
01904
01905 Sleep(1000);
01906 }
01907
01908 printf("\n\nRestoring Orientation to default value.\n\n");
01909
01910
01911 while(mip_filter_tare_orientation(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, 0) != MIP_INTERFACE_OK){}
01912
01913 printf("\n\n");
01914 Sleep(1500);
01915
01917
01919
01920 printf("----------------------------------------------------------------------\n");
01921 printf("Performing Commanded Zero-Velocity Update\n");
01922 printf("----------------------------------------------------------------------\n\n");
01923
01924 while(mip_filter_commanded_zero_velocity_update(&device_interface) != MIP_INTERFACE_OK){}
01925
01926 printf("\n\n");
01927 Sleep(1500);
01928
01930
01932
01933 printf("----------------------------------------------------------------------\n");
01934 printf("Performing Commanded Zero-Angular-Rate Update\n");
01935 printf("----------------------------------------------------------------------\n\n");
01936
01937 while(mip_filter_commanded_zero_angular_rate_update(&device_interface) != MIP_INTERFACE_OK){}
01938
01939 printf("\n\n");
01940 Sleep(1500);
01941
01943
01945
01946 printf("----------------------------------------------------------------------\n");
01947 printf("Cycle through available declination sources\n");
01948 printf("----------------------------------------------------------------------\n\n");
01949
01950
01951 for(i=3; i>=1; i--)
01952 {
01953
01954 declination_source_command = i;
01955
01956 while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &declination_source_command) != MIP_INTERFACE_OK){}
01957
01958
01959 while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_READ, &declination_source_readback) != MIP_INTERFACE_OK){}
01960
01961 if(declination_source_command == declination_source_readback)
01962 {
01963 printf("Declination source successfully set to %d\n", i);
01964 }
01965 else
01966 {
01967 printf("ERROR: Failed to set the declination source to %d!!!\n", i);
01968 }
01969 }
01970
01971 printf("\n\nLoading the default declination source.\n\n");
01972
01973 while(mip_filter_declination_source(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
01974
01975 printf("\n\n");
01976 Sleep(1500);
01977
01978
01980
01982
01983 printf("----------------------------------------------------------------------\n");
01984 printf("Setting the accel magnitude error adaptive measurement values\n");
01985 printf("----------------------------------------------------------------------\n\n");
01986
01987 accel_magnitude_error_command.enable = 0;
01988 accel_magnitude_error_command.low_pass_cutoff = 10;
01989 accel_magnitude_error_command.min_1sigma = 2.0;
01990 accel_magnitude_error_command.low_limit = -2.0;
01991 accel_magnitude_error_command.high_limit = 2.0;
01992 accel_magnitude_error_command.low_limit_1sigma = 4.0;
01993 accel_magnitude_error_command.high_limit_1sigma = 4.0;
01994
01995 while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &accel_magnitude_error_command) != MIP_INTERFACE_OK){}
01996
01997
01998 while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &accel_magnitude_error_readback) != MIP_INTERFACE_OK){}
01999
02000 if((accel_magnitude_error_command.enable == accel_magnitude_error_readback.enable) &&
02001 (abs(accel_magnitude_error_command.low_pass_cutoff - accel_magnitude_error_readback.low_pass_cutoff) < 0.001) &&
02002 (abs(accel_magnitude_error_command.min_1sigma - accel_magnitude_error_readback.min_1sigma) < 0.001) &&
02003 (abs(accel_magnitude_error_command.low_limit - accel_magnitude_error_readback.low_limit) < 0.001) &&
02004 (abs(accel_magnitude_error_command.high_limit - accel_magnitude_error_readback.high_limit) < 0.001) &&
02005 (abs(accel_magnitude_error_command.low_limit_1sigma - accel_magnitude_error_readback.low_limit_1sigma) < 0.001) &&
02006 (abs(accel_magnitude_error_command.high_limit_1sigma - accel_magnitude_error_readback.high_limit_1sigma) < 0.001))
02007 {
02008 printf("accel magnitude error adaptive measurement values successfully set.\n");
02009 }
02010 else
02011 {
02012 printf("ERROR: Failed to set accel magnitude error adaptive measurement values!!!\n");
02013 printf("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", accel_magnitude_error_command.enable,
02014 accel_magnitude_error_command.low_pass_cutoff,
02015 accel_magnitude_error_command.min_1sigma,
02016 accel_magnitude_error_command.low_limit,
02017 accel_magnitude_error_command.high_limit,
02018 accel_magnitude_error_command.low_limit_1sigma,
02019 accel_magnitude_error_command.high_limit_1sigma);
02020
02021 printf("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", accel_magnitude_error_readback.enable,
02022 accel_magnitude_error_readback.low_pass_cutoff,
02023 accel_magnitude_error_readback.min_1sigma,
02024 accel_magnitude_error_readback.low_limit,
02025 accel_magnitude_error_readback.high_limit,
02026 accel_magnitude_error_readback.low_limit_1sigma,
02027 accel_magnitude_error_readback.high_limit_1sigma);
02028 }
02029
02030 printf("\n\nLoading the default accel magnitude error adaptive measurement values.\n\n");
02031
02032 while(mip_filter_accel_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02033
02034 printf("\n\n");
02035 Sleep(1500);
02036
02037
02038
02040
02042
02043 printf("----------------------------------------------------------------------\n");
02044 printf("Setting the mag magnitude error adaptive measurement values\n");
02045 printf("----------------------------------------------------------------------\n\n");
02046
02047 mag_magnitude_error_command.enable = 0;
02048 mag_magnitude_error_command.low_pass_cutoff = 10;
02049 mag_magnitude_error_command.min_1sigma = 0.1;
02050 mag_magnitude_error_command.low_limit = -1.0;
02051 mag_magnitude_error_command.high_limit = 1.0;
02052 mag_magnitude_error_command.low_limit_1sigma = 1.0;
02053 mag_magnitude_error_command.high_limit_1sigma = 1.0;
02054
02055 while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &mag_magnitude_error_command) != MIP_INTERFACE_OK){}
02056
02057
02058 while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &mag_magnitude_error_readback) != MIP_INTERFACE_OK){}
02059
02060 if((mag_magnitude_error_command.enable == mag_magnitude_error_readback.enable) &&
02061 (abs(mag_magnitude_error_command.low_pass_cutoff - mag_magnitude_error_readback.low_pass_cutoff) < 0.001) &&
02062 (abs(mag_magnitude_error_command.min_1sigma - mag_magnitude_error_readback.min_1sigma) < 0.001) &&
02063 (abs(mag_magnitude_error_command.low_limit - mag_magnitude_error_readback.low_limit) < 0.001) &&
02064 (abs(mag_magnitude_error_command.high_limit - mag_magnitude_error_readback.high_limit) < 0.001) &&
02065 (abs(mag_magnitude_error_command.low_limit_1sigma - mag_magnitude_error_readback.low_limit_1sigma) < 0.001) &&
02066 (abs(mag_magnitude_error_command.high_limit_1sigma - mag_magnitude_error_readback.high_limit_1sigma) < 0.001))
02067 {
02068 printf("mag magnitude error adaptive measurement values successfully set.\n");
02069 }
02070 else
02071 {
02072 printf("ERROR: Failed to set mag magnitude error adaptive measurement values!!!\n");
02073 printf("Sent values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_command.enable,
02074 mag_magnitude_error_command.low_pass_cutoff,
02075 mag_magnitude_error_command.min_1sigma,
02076 mag_magnitude_error_command.low_limit,
02077 mag_magnitude_error_command.high_limit,
02078 mag_magnitude_error_command.low_limit_1sigma,
02079 mag_magnitude_error_command.high_limit_1sigma);
02080
02081 printf("Returned values: Enable: %i, Parameters: %f %f %f %f %f %f\n", mag_magnitude_error_readback.enable,
02082 mag_magnitude_error_readback.low_pass_cutoff,
02083 mag_magnitude_error_readback.min_1sigma,
02084 mag_magnitude_error_readback.low_limit,
02085 mag_magnitude_error_readback.high_limit,
02086 mag_magnitude_error_readback.low_limit_1sigma,
02087 mag_magnitude_error_readback.high_limit_1sigma);
02088 }
02089
02090 printf("\n\nLoading the default mag magnitude error adaptive measurement values.\n\n");
02091
02092 while(mip_filter_mag_magnitude_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02093
02094 printf("\n\n");
02095 Sleep(1500);
02096
02097
02098
02099
02101
02103
02104 printf("----------------------------------------------------------------------\n");
02105 printf("Setting the mag dip angle error adaptive measurement values\n");
02106 printf("----------------------------------------------------------------------\n\n");
02107
02108 mag_dip_angle_error_command.enable = 0;
02109 mag_dip_angle_error_command.low_pass_cutoff = 10;
02110 mag_dip_angle_error_command.min_1sigma = 0.1;
02111 mag_dip_angle_error_command.high_limit = 90.0*3.14/180.0;
02112 mag_dip_angle_error_command.high_limit_1sigma = 2.0;
02113
02114 while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &mag_dip_angle_error_command) != MIP_INTERFACE_OK){}
02115
02116
02117 while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_READ, &mag_dip_angle_error_readback) != MIP_INTERFACE_OK){}
02118
02119 if((mag_dip_angle_error_command.enable == mag_magnitude_error_readback.enable) &&
02120 (abs(mag_dip_angle_error_command.low_pass_cutoff - mag_dip_angle_error_readback.low_pass_cutoff) < 0.001) &&
02121 (abs(mag_dip_angle_error_command.min_1sigma - mag_dip_angle_error_readback.min_1sigma) < 0.001) &&
02122 (abs(mag_dip_angle_error_command.high_limit - mag_dip_angle_error_readback.high_limit) < 0.001) &&
02123 (abs(mag_dip_angle_error_command.high_limit_1sigma - mag_dip_angle_error_readback.high_limit_1sigma) < 0.001))
02124 {
02125 printf("mag dip angle error adaptive measurement values successfully set.\n");
02126 }
02127 else
02128 {
02129 printf("ERROR: Failed to set mag dip angle error adaptive measurement values!!!\n");
02130 printf("Sent values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_command.enable,
02131 mag_dip_angle_error_command.low_pass_cutoff,
02132 mag_dip_angle_error_command.min_1sigma,
02133 mag_dip_angle_error_command.high_limit,
02134 mag_dip_angle_error_command.high_limit_1sigma);
02135
02136 printf("Returned values: Enable: %i, Parameters: %f %f %f %f\n", mag_dip_angle_error_readback.enable,
02137 mag_dip_angle_error_readback.low_pass_cutoff,
02138 mag_dip_angle_error_readback.min_1sigma,
02139 mag_dip_angle_error_readback.high_limit,
02140 mag_dip_angle_error_readback.high_limit_1sigma);
02141 }
02142
02143 printf("\n\nLoading the default mag magnitude error adaptive measurement values.\n\n");
02144
02145 while(mip_filter_mag_dip_angle_error_adaptive_measurement(&device_interface, MIP_FUNCTION_SELECTOR_LOAD_DEFAULT, NULL) != MIP_INTERFACE_OK){}
02146
02147 printf("\n\n");
02148 Sleep(1500);
02149
02150
02151
02152
02154
02155
02156
02158
02159
02160
02162
02164
02165 if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_FILTER_DATA_SET, NULL, &filter_packet_callback) != MIP_INTERFACE_OK)
02166 return -1;
02167
02168 if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_AHRS_DATA_SET, NULL, &ahrs_packet_callback) != MIP_INTERFACE_OK)
02169 return -1;
02170
02171 if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_GPS_DATA_SET, NULL, &gps_packet_callback) != MIP_INTERFACE_OK)
02172 return -1;
02173
02174
02175 enable_data_stats_output = 1;
02176
02178
02180
02181 printf("----------------------------------------------------------------------\n");
02182 printf("Setting the AHRS message format\n");
02183 printf("----------------------------------------------------------------------\n\n");
02184
02185 data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED;
02186 data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED;
02187
02188 data_stream_format_decimation[0] = 0x32;
02189 data_stream_format_decimation[1] = 0x32;
02190
02191 data_stream_format_num_entries = 2;
02192
02193 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){}
02194
02195 printf("\n\n");
02196 Sleep(1500);
02197
02199
02201
02202 printf("----------------------------------------------------------------------\n");
02203 printf("Polling AHRS Data.\n");
02204 printf("----------------------------------------------------------------------\n\n");
02205
02206 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){}
02207
02208 printf("\n\n");
02209 Sleep(1500);
02210
02212
02214
02215 printf("----------------------------------------------------------------------\n");
02216 printf("Setting the GPS datastream format\n");
02217 printf("----------------------------------------------------------------------\n\n");
02218
02219 data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS;
02220 data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY;
02221 data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME;
02222
02223 data_stream_format_decimation[0] = 0x04;
02224 data_stream_format_decimation[1] = 0x04;
02225 data_stream_format_decimation[2] = 0x04;
02226
02227 data_stream_format_num_entries = 3;
02228
02229 while(mip_3dm_cmd_gps_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
02230 data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
02231
02232 printf("\n\n");
02233 Sleep(1500);
02234
02236
02238
02239 printf("----------------------------------------------------------------------\n");
02240 printf("Polling GPS Data.\n");
02241 printf("----------------------------------------------------------------------\n\n");
02242
02243 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){}
02244
02245 printf("\n\n");
02246 Sleep(1500);
02247
02249
02251
02252 printf("----------------------------------------------------------------------\n");
02253 printf("Setting the Estimation Filter datastream format\n");
02254 printf("----------------------------------------------------------------------\n\n");
02255
02256 data_stream_format_descriptors[0] = MIP_FILTER_DATA_LLH_POS;
02257 data_stream_format_descriptors[1] = MIP_FILTER_DATA_NED_VEL;
02258 data_stream_format_descriptors[2] = MIP_FILTER_DATA_ATT_EULER_ANGLES;
02259
02260 data_stream_format_decimation[0] = 0x32;
02261 data_stream_format_decimation[1] = 0x32;
02262 data_stream_format_decimation[2] = 0x32;
02263
02264 data_stream_format_num_entries = 3;
02265
02266 while(mip_3dm_cmd_filter_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries,
02267 data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK){}
02268
02269 printf("\n\n");
02270 Sleep(1500);
02271
02273
02275
02276 printf("----------------------------------------------------------------------\n");
02277 printf("Polling Estimation Filter Data.\n");
02278 printf("----------------------------------------------------------------------\n\n");
02279
02280 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){}
02281
02282 printf("\n\n");
02283 Sleep(1500);
02284 }
02285 else
02286 {
02287 printf("ERROR: Standard mode not established\n\n");
02288 }
02289
02290
02291
02292 enable_data_stats_output = 0;
02293
02295
02297
02298 printf("\n----------------------------------------------------------------------\n");
02299 printf("Enable the AHRS datastream\n");
02300 printf("----------------------------------------------------------------------\n\n");
02301
02302 enable = 0x01;
02303
02304 while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02305
02306 printf("\n\n");
02307 Sleep(1500);
02308
02310
02312
02313 printf("----------------------------------------------------------------------\n");
02314 printf("Enable the FILTER datastream\n");
02315 printf("----------------------------------------------------------------------\n\n");
02316
02317 enable = 0x01;
02318
02319 while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_INS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02320
02321 printf("\n\n");
02322 Sleep(1500);
02323
02325
02327
02328 printf("----------------------------------------------------------------------\n");
02329 printf("Enable the GPS datastream\n");
02330 printf("----------------------------------------------------------------------\n");
02331
02332 enable = 0x01;
02333
02334 while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK){}
02335
02336 printf("\n\n");
02337 Sleep(1500);
02338
02340
02342
02343 printf("----------------------------------------------------------------------\n");
02344 printf("Processing incoming packets\n");
02345 printf("----------------------------------------------------------------------\n\n\n");
02346
02347
02348 enable_data_stats_output = 1;
02349
02350 while(1)
02351 {
02352
02353 mip_interface_update(&device_interface);
02354
02355
02356 Sleep(1);
02357 }
02358 }
02359
02360
02361
02363
02364
02365
02367
02368 void filter_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02369 {
02370 mip_field_header *field_header;
02371 u8 *field_data;
02372 u16 field_offset = 0;
02373
02374
02375 switch(callback_type)
02376 {
02378
02380
02381 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02382 {
02383 filter_valid_packet_count++;
02384
02386
02388
02389 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02390 {
02391
02393
02395
02396 switch(field_header->descriptor)
02397 {
02399
02401
02402 case MIP_FILTER_DATA_LLH_POS:
02403 {
02404 memcpy(&curr_filter_pos, field_data, sizeof(mip_filter_llh_pos));
02405
02406
02407 mip_filter_llh_pos_byteswap(&curr_filter_pos);
02408
02409 }break;
02410
02412
02414
02415 case MIP_FILTER_DATA_NED_VEL:
02416 {
02417 memcpy(&curr_filter_vel, field_data, sizeof(mip_filter_ned_velocity));
02418
02419
02420 mip_filter_ned_velocity_byteswap(&curr_filter_vel);
02421
02422 }break;
02423
02425
02427
02428 case MIP_FILTER_DATA_ATT_EULER_ANGLES:
02429 {
02430 memcpy(&curr_filter_angles, field_data, sizeof(mip_filter_attitude_euler_angles));
02431
02432
02433 mip_filter_attitude_euler_angles_byteswap(&curr_filter_angles);
02434
02435 }break;
02436
02437 default: break;
02438 }
02439 }
02440 }break;
02441
02442
02444
02446
02447 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02448 {
02449 filter_checksum_error_packet_count++;
02450 }break;
02451
02453
02455
02456 case MIP_INTERFACE_CALLBACK_TIMEOUT:
02457 {
02458 filter_timeout_packet_count++;
02459 }break;
02460 default: break;
02461 }
02462
02463 print_packet_stats();
02464 }
02465
02466
02468
02469
02470
02472
02473 void ahrs_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02474 {
02475 mip_field_header *field_header;
02476 u8 *field_data;
02477 u16 field_offset = 0;
02478
02479
02480 switch(callback_type)
02481 {
02483
02485
02486 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02487 {
02488 ahrs_valid_packet_count++;
02489
02491
02493
02494 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02495 {
02496
02498
02500
02501 switch(field_header->descriptor)
02502 {
02504
02506
02507 case MIP_AHRS_DATA_ACCEL_SCALED:
02508 {
02509 memcpy(&curr_ahrs_accel, field_data, sizeof(mip_ahrs_scaled_accel));
02510
02511
02512 mip_ahrs_scaled_accel_byteswap(&curr_ahrs_accel);
02513
02514 }break;
02515
02517
02519
02520 case MIP_AHRS_DATA_GYRO_SCALED:
02521 {
02522 memcpy(&curr_ahrs_gyro, field_data, sizeof(mip_ahrs_scaled_gyro));
02523
02524
02525 mip_ahrs_scaled_gyro_byteswap(&curr_ahrs_gyro);
02526
02527 }break;
02528
02530
02532
02533 case MIP_AHRS_DATA_MAG_SCALED:
02534 {
02535 memcpy(&curr_ahrs_mag, field_data, sizeof(mip_ahrs_scaled_mag));
02536
02537
02538 mip_ahrs_scaled_mag_byteswap(&curr_ahrs_mag);
02539
02540 }break;
02541
02542 default: break;
02543 }
02544 }
02545 }break;
02546
02548
02550
02551 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02552 {
02553 ahrs_checksum_error_packet_count++;
02554 }break;
02555
02557
02559
02560 case MIP_INTERFACE_CALLBACK_TIMEOUT:
02561 {
02562 ahrs_timeout_packet_count++;
02563 }break;
02564 default: break;
02565 }
02566
02567 print_packet_stats();
02568 }
02569
02570
02572
02573
02574
02576
02577 void gps_packet_callback(void *user_ptr, u8 *packet, u16 packet_size, u8 callback_type)
02578 {
02579 mip_field_header *field_header;
02580 u8 *field_data;
02581 u16 field_offset = 0;
02582
02583
02584 switch(callback_type)
02585 {
02587
02589
02590 case MIP_INTERFACE_CALLBACK_VALID_PACKET:
02591 {
02592 gps_valid_packet_count++;
02593
02595
02597
02598 while(mip_get_next_field(packet, &field_header, &field_data, &field_offset) == MIP_OK)
02599 {
02600
02602
02604
02605 switch(field_header->descriptor)
02606 {
02608
02610
02611 case MIP_GPS_DATA_LLH_POS:
02612 {
02613 memcpy(&curr_llh_pos, field_data, sizeof(mip_gps_llh_pos));
02614
02615
02616 mip_gps_llh_pos_byteswap(&curr_llh_pos);
02617
02618 }break;
02619
02621
02623
02624 case MIP_GPS_DATA_NED_VELOCITY:
02625 {
02626 memcpy(&curr_ned_vel, field_data, sizeof(mip_gps_ned_vel));
02627
02628
02629 mip_gps_ned_vel_byteswap(&curr_ned_vel);
02630
02631 }break;
02632
02634
02636
02637 case MIP_GPS_DATA_GPS_TIME:
02638 {
02639 memcpy(&curr_gps_time, field_data, sizeof(mip_gps_time));
02640
02641
02642 mip_gps_time_byteswap(&curr_gps_time);
02643
02644 }break;
02645
02646 default: break;
02647 }
02648 }
02649 }break;
02650
02651
02653
02655
02656 case MIP_INTERFACE_CALLBACK_CHECKSUM_ERROR:
02657 {
02658 gps_checksum_error_packet_count++;
02659 }break;
02660
02662
02664
02665 case MIP_INTERFACE_CALLBACK_TIMEOUT:
02666 {
02667 gps_timeout_packet_count++;
02668 }break;
02669 default: break;
02670 }
02671
02672 print_packet_stats();
02673 }
02674
02675
02677
02678
02679
02681
02682 void print_command_line_usage()
02683 {
02684 printf("\n\n");
02685 printf("Usage:\n");
02686 printf("-----------------------------------------------------------------------\n\n");
02687
02688 printf(" GX4-45_Test [com_port_num] [baudrate]\n");
02689 printf("\n\n");
02690 printf(" Example: \"GX4-45_Test 1 115200\", Opens a connection to the \n");
02691 printf(" GX4-45 on COM1, with a baudrate of 115200.\n");
02692 printf("\n\n");
02693 printf(" [ ] - required command input.\n");
02694 printf("\n-----------------------------------------------------------------------\n");
02695 printf("\n\n");
02696 }
02697
02698
02700
02701
02702
02704
02705 void print_header()
02706 {
02707 printf("\n");
02708 printf("GX4-45 Test Program\n");
02709 printf("Copyright 2013. LORD Microstrain\n\n");
02710 }
02711
02712
02714
02715
02716
02718
02719 void print_packet_stats()
02720 {
02721 if(enable_data_stats_output)
02722 {
02723 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,
02724 ahrs_valid_packet_count, ahrs_timeout_packet_count + ahrs_checksum_error_packet_count,
02725 gps_valid_packet_count, gps_timeout_packet_count + gps_checksum_error_packet_count);
02726 }
02727 }
02728
02730
02731
02732
02734
02736
02739
02742
02749
02752
02757
02758 u16 mip_3dm_cmd_hw_specific_imu_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer)
02759 {
02760
02761 gx4_imu_basic_status_field *basic_ptr;
02762 gx4_imu_diagnostic_device_status_field *diagnostic_ptr;
02763 u16 response_size = MIP_FIELD_HEADER_SIZE;
02764
02765 if(status_selector == GX4_IMU_BASIC_STATUS_SEL)
02766 response_size += sizeof(gx4_imu_basic_status_field);
02767 else if(status_selector == GX4_IMU_DIAGNOSTICS_STATUS_SEL)
02768 response_size += sizeof(gx4_imu_diagnostic_device_status_field);
02769
02770 while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){}
02771
02772 if(status_selector == GX4_IMU_BASIC_STATUS_SEL){
02773
02774 if(response_size != sizeof(gx4_imu_basic_status_field))
02775 return MIP_INTERFACE_ERROR;
02776 else if(MIP_SDK_CONFIG_BYTESWAP){
02777
02778 basic_ptr = (gx4_imu_basic_status_field *)response_buffer;
02779
02780 byteswap_inplace(&basic_ptr->device_model, sizeof(basic_ptr->device_model));
02781 byteswap_inplace(&basic_ptr->status_flags, sizeof(basic_ptr->status_flags));
02782 byteswap_inplace(&basic_ptr->system_timer_ms, sizeof(basic_ptr->system_timer_ms));
02783
02784 }
02785
02786 }else if(status_selector == GX4_IMU_DIAGNOSTICS_STATUS_SEL){
02787
02788 if(response_size != sizeof(gx4_imu_diagnostic_device_status_field))
02789 return MIP_INTERFACE_ERROR;
02790 else if(MIP_SDK_CONFIG_BYTESWAP){
02791
02792 diagnostic_ptr = (gx4_imu_diagnostic_device_status_field *)response_buffer;
02793
02794 byteswap_inplace(&diagnostic_ptr->device_model, sizeof(diagnostic_ptr->device_model));
02795 byteswap_inplace(&diagnostic_ptr->status_flags, sizeof(diagnostic_ptr->status_flags));
02796 byteswap_inplace(&diagnostic_ptr->system_timer_ms, sizeof(diagnostic_ptr->system_timer_ms));
02797 byteswap_inplace(&diagnostic_ptr->gyro_range, sizeof(diagnostic_ptr->gyro_range));
02798 byteswap_inplace(&diagnostic_ptr->mag_range, sizeof(diagnostic_ptr->mag_range));
02799 byteswap_inplace(&diagnostic_ptr->pressure_range, sizeof(diagnostic_ptr->pressure_range));
02800 byteswap_inplace(&diagnostic_ptr->temp_degc, sizeof(diagnostic_ptr->temp_degc));
02801 byteswap_inplace(&diagnostic_ptr->last_temp_read_ms, sizeof(diagnostic_ptr->last_temp_read_ms));
02802 byteswap_inplace(&diagnostic_ptr->num_gps_pps_triggers, sizeof(diagnostic_ptr->num_gps_pps_triggers));
02803 byteswap_inplace(&diagnostic_ptr->last_gps_pps_trigger_ms, sizeof(diagnostic_ptr->last_gps_pps_trigger_ms));
02804 byteswap_inplace(&diagnostic_ptr->dropped_packets, sizeof(diagnostic_ptr->dropped_packets));
02805 byteswap_inplace(&diagnostic_ptr->com_port_bytes_written, sizeof(diagnostic_ptr->com_port_bytes_written));
02806 byteswap_inplace(&diagnostic_ptr->com_port_bytes_read, sizeof(diagnostic_ptr->com_port_bytes_read));
02807 byteswap_inplace(&diagnostic_ptr->com_port_write_overruns, sizeof(diagnostic_ptr->com_port_write_overruns));
02808 byteswap_inplace(&diagnostic_ptr->com_port_read_overruns, sizeof(diagnostic_ptr->com_port_read_overruns));
02809
02810 }
02811
02812 }
02813 else
02814 return MIP_INTERFACE_ERROR;
02815
02816 return MIP_INTERFACE_OK;
02817
02818 }
02819
02820
02822
02825
02828
02835
02838
02842
02843 u16 mip_3dm_cmd_hw_specific_device_status(mip_interface *device_interface, u16 model_number, u8 status_selector, u8 *response_buffer)
02844 {
02845
02846 gx4_45_basic_status_field *basic_ptr;
02847 gx4_45_diagnostic_device_status_field *diagnostic_ptr;
02848 u16 response_size = MIP_FIELD_HEADER_SIZE;
02849
02850 if(status_selector == GX4_45_BASIC_STATUS_SEL)
02851 response_size += sizeof(gx4_45_basic_status_field);
02852 else if(status_selector == GX4_45_DIAGNOSTICS_STATUS_SEL)
02853 response_size += sizeof(gx4_45_diagnostic_device_status_field);
02854
02855 while(mip_3dm_cmd_device_status(device_interface, model_number, status_selector, response_buffer, &response_size) != MIP_INTERFACE_OK){}
02856
02857 if(status_selector == GX4_45_BASIC_STATUS_SEL)
02858 {
02859
02860 if(response_size != sizeof(gx4_45_basic_status_field))
02861 return MIP_INTERFACE_ERROR;
02862 else if(MIP_SDK_CONFIG_BYTESWAP){
02863
02864 basic_ptr = (gx4_45_basic_status_field *)response_buffer;
02865
02866 byteswap_inplace(&basic_ptr->device_model, sizeof(basic_ptr->device_model));
02867 byteswap_inplace(&basic_ptr->status_flags, sizeof(basic_ptr->status_flags));
02868 byteswap_inplace(&basic_ptr->system_state, sizeof(basic_ptr->system_state));
02869 byteswap_inplace(&basic_ptr->system_timer_ms, sizeof(basic_ptr->system_timer_ms));
02870
02871 }
02872
02873 }
02874 else if(status_selector == GX4_45_DIAGNOSTICS_STATUS_SEL)
02875 {
02876
02877 if(response_size != sizeof(gx4_45_diagnostic_device_status_field))
02878 return MIP_INTERFACE_ERROR;
02879 else if(MIP_SDK_CONFIG_BYTESWAP){
02880
02881 diagnostic_ptr = (gx4_45_diagnostic_device_status_field *)response_buffer;
02882
02883 byteswap_inplace(&diagnostic_ptr->device_model, sizeof(diagnostic_ptr->device_model));
02884 byteswap_inplace(&diagnostic_ptr->status_flags, sizeof(diagnostic_ptr->status_flags));
02885 byteswap_inplace(&diagnostic_ptr->system_state, sizeof(diagnostic_ptr->system_state));
02886 byteswap_inplace(&diagnostic_ptr->system_timer_ms, sizeof(diagnostic_ptr->system_timer_ms));
02887 byteswap_inplace(&diagnostic_ptr->num_gps_pps_triggers, sizeof(diagnostic_ptr->num_gps_pps_triggers));
02888 byteswap_inplace(&diagnostic_ptr->last_gps_pps_trigger_ms, sizeof(diagnostic_ptr->last_gps_pps_trigger_ms));
02889 byteswap_inplace(&diagnostic_ptr->imu_dropped_packets, sizeof(diagnostic_ptr->imu_dropped_packets));
02890 byteswap_inplace(&diagnostic_ptr->gps_dropped_packets, sizeof(diagnostic_ptr->gps_dropped_packets));
02891 byteswap_inplace(&diagnostic_ptr->filter_dropped_packets, sizeof(diagnostic_ptr->filter_dropped_packets));
02892 byteswap_inplace(&diagnostic_ptr->com1_port_bytes_written, sizeof(diagnostic_ptr->com1_port_bytes_written));
02893 byteswap_inplace(&diagnostic_ptr->com1_port_bytes_read, sizeof(diagnostic_ptr->com1_port_bytes_read));
02894 byteswap_inplace(&diagnostic_ptr->com1_port_write_overruns, sizeof(diagnostic_ptr->com1_port_write_overruns));
02895 byteswap_inplace(&diagnostic_ptr->com1_port_read_overruns, sizeof(diagnostic_ptr->com1_port_read_overruns));
02896 byteswap_inplace(&diagnostic_ptr->imu_parser_errors, sizeof(diagnostic_ptr->imu_parser_errors));
02897 byteswap_inplace(&diagnostic_ptr->imu_message_count, sizeof(diagnostic_ptr->imu_message_count));
02898 byteswap_inplace(&diagnostic_ptr->imu_last_message_ms, sizeof(diagnostic_ptr->imu_last_message_ms));
02899 byteswap_inplace(&diagnostic_ptr->gps_parser_errors, sizeof(diagnostic_ptr->gps_parser_errors));
02900 byteswap_inplace(&diagnostic_ptr->gps_message_count, sizeof(diagnostic_ptr->gps_message_count));
02901 byteswap_inplace(&diagnostic_ptr->gps_last_message_ms, sizeof(diagnostic_ptr->gps_last_message_ms));
02902 }
02903 }
02904 else
02905 return MIP_INTERFACE_ERROR;
02906
02907 return MIP_INTERFACE_OK;
02908
02909 }