int main(int argc, char** argv) { u32 com_port, baudrate; base_device_info_field device_info; u8 temp_string[20] = {0}; s16 i; u16 device_descriptors[128] = {0}; u16 device_descriptors_size = 128*2; u32 bit_result; u8 com_mode = 0; signal(SIGINT, sigHandler); if(argc != NUM_COMMAND_LINE_ARGUMENTS) { //print_command_line_usage(); return -1; } printf("Initializing interface..."); fflush(stdout); if(mip_interface_init(com_port, baudrate, &device_interface, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK) return -1; printf("done\n"); if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_AHRS_DATA_SET, NULL, &ahrs_packet_callback) != MIP_INTERFACE_OK) return -1; /// //Wait for packets to arrive /// printf("in the while loop\n"); while(m_running > 0) { //Update the parser (this function reads the port and parses the bytes mip_interface_update(&device_interface); //Be nice to other programs //usleep(10); } printf("Closing interface..."); fflush(stdout); if (mip_interface_close(&device_interface) != MIP_INTERFACE_OK) { printf("Failed to close the interface\n"); } printf("done\n"); return 0; }
int main(int argc, char* argv[]) { u32 com_port, baudrate; base_device_info_field device_info; u8 temp_string[20] = {0}; u32 bit_result; u8 enable = 1; u8 data_stream_format_descriptors[10] = {0}; u16 data_stream_format_decimation[10] = {0}; u8 data_stream_format_num_entries = 0; u8 readback_data_stream_format_descriptors[10] = {0}; u16 readback_data_stream_format_decimation[10] = {0}; u8 readback_data_stream_format_num_entries = 0; u16 base_rate = 0; u16 device_descriptors[128] = {0}; u16 device_descriptors_size = 128*2; s16 i; u8 com_mode = 0; u8 datastream_format = 0; u8 gps_dynamics_mode = 0; u8 ahrs_time_selector = 0; u8 readback_ahrs_time_selector = 0; u32 ahrs_time_value = 0; mip_ahrs_signal_settings signal_conditioning_settings = {0}, readback_signal_conditioning_settings = {0}; u8 power_state = 0, readback_power_state = 0; gx3_35_device_status_field gx3_35_status; u16 gx3_35_status_size = sizeof(gx3_35_device_status_field); /// //Verify the command line arguments /// if(argc != NUM_COMMAND_LINE_ARGUMENTS) { print_command_line_usage(); return -1; } //Convert the arguments com_port = atoi(argv[1]); baudrate = atoi(argv[2]); /// //Initialize the interface to the device /// if(mip_interface_init(com_port, baudrate, &device_interface, DEFAULT_PACKET_TIMEOUT_MS) != MIP_INTERFACE_OK) return -1; //////////////////////////////////////////////////////////////////////////////////// // //Base Command Tests // //////////////////////////////////////////////////////////////////////////////////// /// //Put the GX3-35 into idle mode /// printf("\n\nPutting Device Into Idle Mode....\n"); while(mip_base_cmd_idle(&device_interface) != MIP_INTERFACE_OK) { } /// //Try to ping the GX3-35 /// printf("\n\nPinging Device....\n"); while(mip_base_cmd_ping(&device_interface) != MIP_INTERFACE_OK) { } /// //Get the device information /// printf("\n\nGetting Device Information....\n"); while(mip_base_cmd_get_device_info(&device_interface, &device_info) != MIP_INTERFACE_OK) { } printf("\n\nDevice Info:\n"); printf("---------------------------------------------\n"); memcpy(temp_string, device_info.model_name, BASE_DEVICE_INFO_PARAM_LENGTH*2); printf("Model Name => %s\n", temp_string); memcpy(temp_string, device_info.model_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); printf("Model Number => %s\n", temp_string); memcpy(temp_string, device_info.serial_number, BASE_DEVICE_INFO_PARAM_LENGTH*2); printf("Serial Number => %s\n", temp_string); memcpy(temp_string, device_info.lotnumber, BASE_DEVICE_INFO_PARAM_LENGTH*2); printf("Lot Number => %s\n", temp_string); memcpy(temp_string, device_info.device_options, BASE_DEVICE_INFO_PARAM_LENGTH*2); printf("Options => %s\n", temp_string); printf("Firmware Version => %d.%d.%d\n\n", (device_info.firmware_version)/1000, (device_info.firmware_version)%1000/100, (device_info.firmware_version)%100); /// //Get the supported descriptors /// printf("\n\nGetting Supported descriptors....\n"); while(mip_base_cmd_get_device_supported_descriptors(&device_interface, (u8*)device_descriptors, &device_descriptors_size) != MIP_INTERFACE_OK) { } printf("\n\nSupported descriptors:\n\n"); for(i=0; i< device_descriptors_size/2; i++) { printf("Descriptor Set: %02x, Descriptor: %02x\n", device_descriptors[i] >> 8, device_descriptors[i]&0xFF); } /// //Peform a built-in-test /// printf("\n\nRunning Built In Test....\n"); while(mip_base_cmd_built_in_test(&device_interface, &bit_result) != MIP_INTERFACE_OK) { } printf("\nBIT Result (should be 0x00000000) => 0x%08x\n\n", bit_result); usleep(2000); //////////////////////////////////////////////////////////////////////////////////// // //System Command Tests // //////////////////////////////////////////////////////////////////////////////////// /// //Set/Read the current com mode /// printf("\n\nCycle through available com modes....\n\n"); //Loop through valid modes and set/read current come mode for(i=3; i>=1; i--) { //Set the com mode com_mode = i; while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &com_mode) != MIP_INTERFACE_OK) { } //Reset the com_mode variable com_mode = 0; //Read back the com mode while(mip_system_com_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &com_mode) != MIP_INTERFACE_OK) { } if(com_mode == i) { printf("Com mode successfully set to %d\n", com_mode); } else { printf("ERROR: Failed to set com mode to %d!!!\n", com_mode); } } //////////////////////////////////////////////////////////////////////////////////// // //3DM Command Tests // //////////////////////////////////////////////////////////////////////////////////// /// //Set/Read the AHRS power state /// printf("\n\nTurn off/on AHRS and verify state....\n\n"); //Loop through valid modes and set/read current come mode for(i=0; i<=1; i++) { //Set the power state (off, then on) if(i == 0) power_state = MIP_3DM_POWER_STATE_OFF; else if(i == 1) power_state = MIP_3DM_POWER_STATE_ON; while(mip_3dm_cmd_power_state(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_POWER_STATE_DEVICE_AHRS, &power_state) != MIP_INTERFACE_OK) { } //Reset the readback_power_state variable readback_power_state = 0; //Read back the com mode while(mip_3dm_cmd_power_state(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_POWER_STATE_DEVICE_AHRS, &readback_power_state) != MIP_INTERFACE_OK) { } if(power_state == readback_power_state) { printf("Power state successfully set to %d\n", power_state); } else { printf("ERROR: Failed to set power state to %d!!!\n", power_state); } } /// //Set/Read the GPS power state /// printf("\n\nTurn off/on GPS and verify state....\n\n"); //Loop through valid modes and set/read current come mode for(i=0; i<=1; i++) { //Set the power state (off, then on) if(i == 0) power_state = MIP_3DM_POWER_STATE_OFF; else if(i == 1) power_state = MIP_3DM_POWER_STATE_ON; while(mip_3dm_cmd_power_state(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_POWER_STATE_DEVICE_GPS, &power_state) != MIP_INTERFACE_OK) { } //Reset the readback_power_state variable readback_power_state = 0; //Read back the com mode while(mip_3dm_cmd_power_state(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_POWER_STATE_DEVICE_GPS, &readback_power_state) != MIP_INTERFACE_OK) { } if(power_state == readback_power_state) { printf("Power state successfully set to %d\n", power_state); } else { printf("ERROR: Failed to set power state to %d!!!\n", power_state); } } /// //Get GX3-35 diagnostic status packet /// printf("\n\nGetting the GX3-35 diagnostic status packet....\n"); while(mip_3dm_cmd_device_status(&device_interface, GX3_35_MODEL_NUMBER, GX3_35_DIAGNOSTICS_STATUS_SEL, (u8*)&gx3_35_status, &gx3_35_status_size) != MIP_INTERFACE_OK) { } /// //Get AHRS Base Rate /// printf("\n\nGetting the AHRS datastream base rate....\n"); while(mip_3dm_cmd_get_ahrs_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK) { } printf("\nAHRS Base Rate => %d Hz\n", base_rate); /// //Get GPS Base Rate /// printf("\n\nGetting the GPS datastream base rate....\n"); while(mip_3dm_cmd_get_gps_base_rate(&device_interface, &base_rate) != MIP_INTERFACE_OK) { } printf("\nGPS Base Rate => %d Hz\n", base_rate); /// //Poll for an AHRS packet /// printf("\n\nPolling for an AHRS packet....\n\n"); data_stream_format_descriptors[0] = 0x04; data_stream_format_descriptors[1] = 0x05; data_stream_format_num_entries = 2; //Poll for the AHRS packet 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) { } printf("Got an ACK.\n"); /// //Poll for a GPS packet /// printf("\n\nPolling for a GPS packet....\n\n"); data_stream_format_descriptors[0] = 0x03; data_stream_format_descriptors[1] = 0x05; data_stream_format_descriptors[2] = 0x08; data_stream_format_num_entries = 3; //Poll for the GPS packet 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) { } printf("Got an ACK.\n"); /// //Setup the AHRS message format and verify via read-back /// printf("\n\nSetting the AHRS datastream format....\n\n"); data_stream_format_descriptors[0] = MIP_AHRS_DATA_ACCEL_SCALED; data_stream_format_descriptors[1] = MIP_AHRS_DATA_GYRO_SCALED; data_stream_format_descriptors[2] = MIP_AHRS_DATA_MAG_SCALED; data_stream_format_decimation[0] = 0x0A; data_stream_format_decimation[1] = 0x0A; data_stream_format_decimation[2] = 0x0A; data_stream_format_num_entries = 3; //Set the message format 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) { } //On entry to the read, we set the maximum number of entries we can accept readback_data_stream_format_num_entries = 10; //Read the message format while(mip_3dm_cmd_ahrs_message_format(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_data_stream_format_num_entries, readback_data_stream_format_descriptors, readback_data_stream_format_decimation) != MIP_INTERFACE_OK) { } //Compare the message formats if(data_stream_format_num_entries == readback_data_stream_format_num_entries) { printf("Number of fields match: %d fields\n", data_stream_format_num_entries); } else { printf("ERROR: Number of fields mismatch: %d fields written, %d fields read!!!\n", data_stream_format_descriptors, readback_data_stream_format_descriptors); } for(i=0; i<data_stream_format_num_entries; i++) { if((data_stream_format_descriptors[i] == readback_data_stream_format_descriptors[i]) && (data_stream_format_decimation[i] == readback_data_stream_format_decimation[i])) { printf("Descriptor information for field %d matches\n", i); } else { printf("ERROR: Descriptor information for field %d mismatch!!!! Written: %d %d, Read: %d %d\n", i, data_stream_format_descriptors[i], data_stream_format_decimation[i], readback_data_stream_format_descriptors[i], readback_data_stream_format_decimation[i]); } } /// //Setup the GPS message format and verify via read-back /// printf("\n\nSetting the GPS datastream format....\n\n"); data_stream_format_descriptors[0] = MIP_GPS_DATA_LLH_POS; data_stream_format_descriptors[1] = MIP_GPS_DATA_NED_VELOCITY; data_stream_format_descriptors[2] = MIP_GPS_DATA_GPS_TIME; data_stream_format_decimation[0] = 0x01; data_stream_format_decimation[1] = 0x01; data_stream_format_decimation[2] = 0x01; data_stream_format_num_entries = 3; while(mip_3dm_cmd_gps_message_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &data_stream_format_num_entries, data_stream_format_descriptors, data_stream_format_decimation) != MIP_INTERFACE_OK) { } //On entry to the read, we set the maximum number of entries we can accept readback_data_stream_format_num_entries = 10; //Read the message format while(mip_3dm_cmd_gps_message_format(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_data_stream_format_num_entries, readback_data_stream_format_descriptors, readback_data_stream_format_decimation) != MIP_INTERFACE_OK) { } //Compare the message formats if(data_stream_format_num_entries == readback_data_stream_format_num_entries) { printf("Number of fields match: %d fields\n", data_stream_format_num_entries); } else { printf("ERROR: Number of fields mismatch: %d fields written, %d fields read!!!\n", data_stream_format_num_entries, readback_data_stream_format_num_entries); } for(i=0; i<data_stream_format_num_entries; i++) { if((data_stream_format_descriptors[i] == readback_data_stream_format_descriptors[i]) && (data_stream_format_decimation[i] == readback_data_stream_format_decimation[i])) { printf("Descriptor information for field %d matches\n", i); } else { printf("ERROR: Descriptor information for field %d mismatch!!!! Written: %d %d, Read: %d %d\n", i, data_stream_format_descriptors[i], data_stream_format_decimation[i], readback_data_stream_format_descriptors[i], readback_data_stream_format_decimation[i]); } } /// //Set/Read the AHRS datastream format /// printf("\n\nCycle through available AHRS datastream formats....\n\n"); //Loop through valid modes and set/read current come mode for(i=2; i>=1; i--) { //Set the datastream format datastream_format = i; while(mip_3dm_cmd_datastream_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &datastream_format) != MIP_INTERFACE_OK) { } //Reset the datastream_format variable datastream_format = 0; //Read back the com mode while(mip_3dm_cmd_datastream_format(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_AHRS_DATASTREAM, &datastream_format) != MIP_INTERFACE_OK) { } if(datastream_format == i) { printf("Datastream format successfully set to %d\n", datastream_format); } else { printf("ERROR: Failed to set datastream format to %d!!!\n", datastream_format); } } /// //Set/Read the GPS datastream format /// printf("\n\nCycle through available GPS datastream formats....\n\n"); //Loop through valid modes and set/read current come mode for(i=2; i>=1; i--) { //Set the datastream format datastream_format = i; while(mip_3dm_cmd_datastream_format(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &datastream_format) != MIP_INTERFACE_OK) { } //Reset the datastream_format variable datastream_format = 0; //Read back the com mode while(mip_3dm_cmd_datastream_format(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_GPS_DATASTREAM, &datastream_format) != MIP_INTERFACE_OK) { } if(datastream_format == i) { printf("Datastream format successfully set to %d\n", datastream_format); } else { printf("ERROR: Failed to set datastream format to %d!!!\n", datastream_format); } } /// //Set/Read the GPS dynamics mode /// printf("\n\nCycle through available GPS Dynamics Modes....\n\n"); //Loop through valid modes and set/read current come mode for(i=8; i>=0; i--) { //Skip 1, per DCP if(i != 1) { //Set the dynamics mode gps_dynamics_mode = i; while(mip_3dm_cmd_gps_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &gps_dynamics_mode) != MIP_INTERFACE_OK) { } //Reset the gps_dynamics_mode variable gps_dynamics_mode = 0; //Read back the com mode while(mip_3dm_cmd_gps_dynamics_mode(&device_interface, MIP_FUNCTION_SELECTOR_READ, &gps_dynamics_mode) != MIP_INTERFACE_OK) { } if(gps_dynamics_mode == i) { printf("GPS dynamics mode successfully set to %d\n", gps_dynamics_mode); } else { printf("ERROR: Failed to set GPS dynamics mode to %d!!!\n", gps_dynamics_mode); } } } /// //Set/Read the AHRS signal conditioning settings /// printf("\n\nSet the AHRS signal conditioning settings....\n\n"); //Read the signal conditioning settings (for a starting point) while(mip_3dm_cmd_ahrs_signal_conditioning(&device_interface, MIP_FUNCTION_SELECTOR_READ, &signal_conditioning_settings) != MIP_INTERFACE_OK) { } //Set the signal conditioning settings signal_conditioning_settings.mag_filter_width = 32; signal_conditioning_settings.inertial_filter_width = 32; //Write the signal conditioning settings while(mip_3dm_cmd_ahrs_signal_conditioning(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &signal_conditioning_settings) != MIP_INTERFACE_OK) { } //Read back the signal conditioning settings while(mip_3dm_cmd_ahrs_signal_conditioning(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_signal_conditioning_settings) != MIP_INTERFACE_OK) { } if((readback_signal_conditioning_settings.mag_filter_width == signal_conditioning_settings.mag_filter_width) && (readback_signal_conditioning_settings.inertial_filter_width == signal_conditioning_settings.inertial_filter_width)) { printf("Signal conditioning settings successfully set\n"); } else { printf("ERROR: Failed to set signal conditioning settings!!!\n"); } /// //Set/Read the AHRS timestamp format /// printf("\n\nCycle through available AHRS timestamp format values....\n\n"); //Loop through valid modes and set/read current come mode for(i=2; i>=1; i--) { //Set the enable flag ahrs_time_selector = i; while(mip_3dm_cmd_ahrs_timestamp(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, &ahrs_time_selector, &ahrs_time_value) != MIP_INTERFACE_OK) { } readback_ahrs_time_selector = i; //Read back the com mode while(mip_3dm_cmd_ahrs_timestamp(&device_interface, MIP_FUNCTION_SELECTOR_READ, &readback_ahrs_time_selector, &ahrs_time_value) != MIP_INTERFACE_OK) { } printf("Timestamp format test successful with selector %d\n", ahrs_time_selector); } /// //Set/Read the AHRS continuous datastream enable /// printf("\n\nCycle through available AHRS continuous datastream enable values....\n\n"); //Loop through valid modes and set/read current come mode for(i=0; i<=1; i++) { //Set the enable flag enable = i; while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { } //Reset the enable variable enable = 0; //Read back the com mode while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_AHRS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { } if(enable == i) { printf("Continuous datastream enable successfully set to %d\n", enable); } else { printf("ERROR: Failed to set continuous datastream enable to %d!!!\n", enable); } } /// //Set/Read the GPS continuous datastream enable /// printf("\n\nCycle through available GPS continuous datastream enable values....\n\n"); //Loop through valid modes and set/read current come mode for(i=0; i<=1; i++) { //Set the enable flag enable = i; while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_WRITE, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { } //Reset the enable variable enable = 0; //Read back the com mode while(mip_3dm_cmd_continuous_data_stream(&device_interface, MIP_FUNCTION_SELECTOR_READ, MIP_3DM_GPS_DATASTREAM, &enable) != MIP_INTERFACE_OK) { } if(enable == i) { printf("\nContinuous datastream enable successfully set to %d\n", enable); } else { printf("\nERROR: Failed to set continuous datastream enable to %d!!!\n", enable); } } /// //Resume the GX3-35 datastreams /// printf("\n\nResuming device datastreams....\n"); while(mip_base_cmd_resume(&device_interface) != MIP_INTERFACE_OK) { } printf("\n\n"); /// //Setup the GX3-35 dataset callbacks (AHRS, GPS), note: no user data is required for this example so we set the pointer to NULL. /// if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_AHRS_DATA_SET, NULL, &ahrs_packet_callback) != MIP_INTERFACE_OK) return -1; if(mip_interface_add_descriptor_set_callback(&device_interface, MIP_GPS_DATA_SET, NULL, &gps_packet_callback) != MIP_INTERFACE_OK) return -1; /// //Wait for packets to arrive /// while(1) { //Update the parser (this function reads the port and parses the bytes mip_interface_update(&device_interface); //Be nice to other programs usleep(1); } }