void DCTFFTW::DCTBytes2D(const unsigned char *srcp, int src_pitch, unsigned char *dctp, int dct_pitch)
{
_asm emms;
Bytes2Float (srcp, src_pitch, fSrc);
fftwf_execute_r2r_addr(dctplan, fSrc, fSrcDCT);
Float2Bytes (dctp, dct_pitch, fSrcDCT);
}
// CommandDialog
// Prompts user for device commands and returns reply from device
int CommandDialog(ComPortHandle comPort, unsigned int command){
// unsigned int command;
unsigned char ccommand;
int i=0;
int size;
Byte response[4096] = {0};
// printf("\nEnter command in hexadecimal format, valid commands range from c1 to fe (00 to EXIT)\n");
// printf("(SEE: 3DM-GX3® Data Communications Protocol Manual for more information):\n");
// scanf("%x", &command);//takes 1 byte command in hexadecimal format
// printf("%x 1\n", command);
// command = 0xdf;
// printf("%x 2\n", command);
ccommand=(char)command;
if(command==0x00)//command to exit program
return FALSE;
else
writeComPort(comPort, &ccommand, 1);//write command to port
//getchar();//flush keyboard buffer
Purge(comPort);//flush port
size = readComPort(comPort, &response[0], 4096);
if(size<=0){
printf("No data read from previous command.\n");
return TRUE;
}
else{
// printf("Data returned from device:\n");
while(size>0){//loop to read until no more bytes in read buffer
if(size<0){
printf("BAD READ\n");
return TRUE;
}
else{
/* --------------------------Print Command 0xD2 Numerical Values------------------------------- */
//Format: [stabAccelX],[stabAccelY],[stabAccelZ],[angRateX],[angRateY],[angRateZ],[stabMagX],[stabMagY],[stabMagZ],[heading],[timestamp]
D2_Stab_AAM D2_Data;
for(i=0;i<3;i++)
{
D2_Data.StabAccel[i] = Bytes2Float(&response[1 + i*4]);
D2_Data.AngRate[i] = Bytes2Float(&response[13 + i*4]);
D2_Data.StabMag[i] = Bytes2Float(&response[25 + i*4]);
}
printf("%f,%f,%f,", D2_Data.StabAccel[0], D2_Data.StabAccel[1], D2_Data.StabAccel[2]);
printf("%f,%f,%f,", (180.0/PI)*D2_Data.AngRate[0], (180.0/PI)*D2_Data.AngRate[1], (180.0/PI)*D2_Data.AngRate[2]);
printf("%f,%f,%f,", D2_Data.StabMag[0], D2_Data.StabMag[1], D2_Data.StabMag[2]);
float north;
if(D2_Data.StabMag[1] > 0) {
north = 90.0-(180.0/PI)*atan(D2_Data.StabMag[0]/D2_Data.StabMag[1]);
}
if(D2_Data.StabMag[1] < 0) {
north = 270.0-(180.0/PI)*atan(D2_Data.StabMag[0]/D2_Data.StabMag[1]);
}
if(D2_Data.StabMag[1]==0 && D2_Data.StabMag[0]<0) {
north = 180.0;
}
if(D2_Data.StabMag[1]==0 && D2_Data.StabMag[0]>0) {
north = 0.0;
}
north = 360.0-north;
D2_Data.timer = Bytes2Ulong(&response[37]);
float sec;
sec = (float) (D2_Data.timer/262144.0);
printf("%f,", north);
printf("%f\n", sec);
/* -------------------------------------------------------------------------------------------- */
return TRUE;
}
size = readComPort(comPort, &response[0], 4096);
}
}
}
