void SamplingData(int portNum){
int Value=0;
unsigned short DCcheck=0;
BOOL bQuit = FALSE;
BOOL bPrintHeader = TRUE;
BYTE Record[20];
BYTE BlittleEndian[31];
BYTE byteOne=0x00;
BYTE byteTwo=0x00;
int chOption = 0;
short fw_check = 0;
int i = 0;
float LEaccel[4];
float LEang[4];
while(Value == 0)
{
if(bPrintHeader)
{
printf("\n");
printf("Select an Option From the Following Menu \n");
printf("0 Data Rate - Modify or review the current DataRate\n");
printf("1 Calculate Orientation - Enable Calculate Orientation \n");
printf("2 Coning & Sculling - Enable Coning & Sculling \n");
printf("3 Little Endian - Change IMU Big Endian format to Little\n");
printf("4 NaN's Suppression - Suppress NaN Data \n");
printf("5 Finite Size Corrction - Enable Finite Size Correction \n");
printf("6 Disable Magnetometer - Disable the Magnetometer \n");
printf("7 Enable Magnetometer - Enable the Magnetometer Low Power Setting \n");
printf("8 Disable Magnetic North - Disable the Magnetic North Compenstation \n");
printf("9 Disable Gravity Comp - Disable the Gravity Compenstation \n");
printf("Q Enable Quaternion - Enable the Quaternion Calculation \n");
printf("E Exit Sampling - Exit the Sampling Menu and return to the Main Menu\n");
bPrintHeader = FALSE;
}
//read option from the keyboard
while(!ReadCharNoReturn(&chOption))
{
Sleep(50);
}
switch (chOption){
case '0':
SetDataRate(portNum); bPrintHeader = TRUE; break;
case '1': //Enable Calculate Orientation
//DataConditioning = 1;
byteOne=0x00;
byteTwo = 0x01;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
//DCcheck = convert2short(&Record[3]);
if(byteTwo == Record[4])
printf("\nSample Setting Calculate Orientation now Enabled.\n");
else
printf("\nError: Failure to Enable Sample Setting Calculate Orientation.\n");
bPrintHeader = TRUE; break;
case '2': //Enable Coning & Sculling
byteOne=0x00;
byteTwo = 0x02;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteTwo == Record[4])
printf("\nSample Setting Coning & Sculling now Enabled.\n");
else
printf("\nError: Failure to Enable Sample Setting Coning & Sculling.\n");
bPrintHeader = TRUE; break;
case '3': //Set Floating Point from sensor to Little Endian
byteOne=0x00;
byteTwo = 0x10;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteTwo == Record[4]){
printf("\n\nSample Setting Floating Point data is sent in Little Endian.\n");
i3dmgx3_AccelAndAngRate(portNum,&BlittleEndian[0]);
//printf("received Data----------------\n");
for(i=0;i<4;i++){
LEaccel[i] = Little_Endian_Float(&BlittleEndian[1 + i*4]);
//LEaccel[0] = FloatFromBytes(BlittleEndian[1 ]);
LEang[i] = Little_Endian_Float(&BlittleEndian[13 + i*4]);
}
printf("Acceleration & Angular Rate (0xC2)______\n");
printf(" X Y Z \n");
//printf(" %2.6f\n ",LEaccel[0]);
printf(" Accel %2.6f\t%2.6f\t%2.6f\n", LEaccel[0], LEaccel[1], LEaccel[2]);
printf(" Angle %2.6f\t%2.6f\t%2.6f\n", LEang[0], LEang[1], LEang[2]);
}else
printf("Error: Failure to set Sample Setting Floating Point data to Little Endian Format.\n");
bPrintHeader = TRUE; break;
case '4': //Suppress NaN data
byteOne=0x00;
byteTwo = 0x20;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteTwo == Record[4])
printf("\nSample Setting NaN data is now suppressed.\n");
else
printf("\nError: Failure to suppress Sample Setting NaN data. \n");
bPrintHeader = TRUE; break;
case '5': //Enable Finite Size Correction
byteOne=0x00;
byteTwo = 0x40;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteTwo == Record[4])
printf("\nSample Setting Finite Size Correction now Enabled.\n");
else
printf("\nError: Failure to Enable Sample Setting Finite Size Correction.\n");
bPrintHeader = TRUE; break;
//Case(s) 6 - 9 and q require firmware 1.1.27 or greater --------------
case '6': //Disable Magnetometer
byteOne=0x01;
byteTwo = 0x00;
fw_check = 1;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteOne == Record[3])
printf("\nSample Setting Magnetometer Disabled. \n");
else
printf("\nError: Failure to Disable Sample Setting Magnetometer.\n");
bPrintHeader = TRUE; break;
case '7': //Enable Magnetometer low power
byteOne=0x02;
byteTwo = 0x00;
fw_check = 1;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteOne == Record[3])
printf("\nSample Setting Magnetometer now Enabled to Low Power Setting.\n");
else
printf("\nError: Failure to Enable Sample Setting Magnetometer Low Power Setting.\n");
bPrintHeader = TRUE; break;
case '8': //Disable Magnetic North Compensation
byteOne = 0x04; //To Enable use byteOne=0x00;
byteTwo = 0x00;
fw_check = 1; // to enable use fw_check =8 to set time
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteOne == Record[3])
printf("\nSample Setting Magnetic North Compensationn Disabled.");
else
printf("\nError: Failure to Disable Sample Setting Magnetic North Compensation.\n");
// if selection is Enabled ask if user wants to run realign up and north 0xDD
bPrintHeader = TRUE; break;
case '9': //Disable Gravity Compensationn
byteOne = 0x08; //To Enable use byteOne=0x00;
byteTwo = 0x00;
fw_check = 1; // to enable use fw_check =9 to set time
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteOne == Record[3])
printf("\nSample Setting Gravity Compensationn Disabled");
else
printf("\nError: Failure to Disable Sample Setting Gravity Compensation\n");
// if selection is Enabled ask if user wants to run realign up and north 0xDD
bPrintHeader = TRUE; break;
case 'q':
case 'Q'://Enable Quaternion(s)
byteOne=0x10; //needs bit 0 bit 1 and bit 11 set on
byteTwo=0x03;
fw_check = 1;
SetSampleSetting(portNum,byteOne, byteTwo, &Record[0], fw_check);
if(byteTwo == Record[4]){
printf("\nSample Setting Quaternion Calculation now enabled\n");
Get_Quaternion(portNum);
}
else
printf("\nError: Failure to set Sample Setting Quaternion Calculation\n");
//Get_Quaternion(portNum);
bPrintHeader = TRUE; break;
case 'e':
case 'E': bQuit = TRUE; bPrintHeader = FALSE; return; break;
case 'h':
case 'H': bPrintHeader = TRUE; break;
default: printf("Invalid Option\n\n"); bPrintHeader = TRUE;
}
}
}
/*--------------------------------------------------------------------*/
int main(int argc, char **argv) {
s32 zvert=0;
BOOL endloopy = FALSE;
s16 portNum;
s16 deviceNum = 0;
s16 i;
s16 Ccount=0;
u16 value=0;
s16 id_flag = 0;
s16 errorCode;
s16 tryPortNum = 1;
unsigned char Record[79]; //record returned from device read where max size is 79
C2Accel_AngRecord Accel_AngRecord;
printf("\n 3DM-GX3 Read Acceleration and Angular Rate\n");
/*-------- If user specifies a port, then use it */
if (argc > 1) {
tryPortNum = atoi(argv[1]);
if (tryPortNum < 2 || tryPortNum > 256) {
printf(" usage: i3dmgx3 <portNumber>\n");
printf(" valid ports are 2..256\n");
exit(1);
}
/*-------- open a port, map a device */
portNum = i3dmgx3_openPort(tryPortNum, 115200, 8, 0, 1, 1024, 1024);
if (portNum<0) {
printf(" port open failed.\n");
printf(" Comm error %d, %s: ", portNum, explainError(portNum));
goto Exit;
}
}else{
portNum=OnGetSerialPorts();
if(portNum<0)
goto Exit;
}
printf("\n Using COM Port #%d \n", portNum);
/*-------- Set Comm Timeout values */
errorCode = setCommTimeouts(portNum, 50, 50); /* Read & Write timeout values */
if (errorCode!=I3DMGX3_COMM_OK) {
printf(" setCommTimeouts failed on port:%d with errorcode:%d\n",portNum,errorCode);
goto Exit;
}
/*-------- Disclose the byte order of host */
if( TestByteOrder() !=BIG_ENDIAN)
printf(" (Local Host is in Little Endian format)\n");
else
printf(" (Local Host is in Big Endian format)\n");
printf("\n");
/*-------- 0xC2 Accel and Ang rate Output --- Accel x y z and Ang x y z */
printf("\n 0xC2 Accel and Ang Output \n");
errorCode = i3dmgx3_AccelAndAngRate(portNum, &Record[0]);
if (errorCode < 0){
printf(" Error Accel and AngRate - : %s\n", explainError(errorCode));
endloopy =TRUE;
}else{
for (i=0; i<3; i++) {
Accel_AngRecord.Accel[i] = FloatFromBytes(&Record[1 + i*4]); // extract float from byte array
Accel_AngRecord.AngRt[i] = FloatFromBytes(&Record[13 + i*4]); // extract float from byte array
}
printf("\n\tAccel X\t\tAccel Y\t\tAccel Z\n");
printf(" \t%f\t%f\t%f\n", Accel_AngRecord.Accel[0], Accel_AngRecord.Accel[1], Accel_AngRecord.Accel[2]);
printf("\n\t Ang X\t\t Ang Y\t\t Ang Z\n");
printf(" \t%f\t%f\t%f\n", Accel_AngRecord.AngRt[0], Accel_AngRecord.AngRt[1], Accel_AngRecord.AngRt[2]);
Accel_AngRecord.timer = convert2ulong(&Record[25]);
printf("\n Time Stamp: %u\n", Accel_AngRecord.timer);
}
Exit:
/*-------- close device */
if (portNum >= 0)
i3dmgx3_closeDevice(portNum);
/*-------- wait for user to respond before exiting */
printf("\nHit return to exit...\n");
while (getchar() == EOF);
return(0);
}
