// dac_adc.cpp : Defines the entry point for the console application. // #include "stdafx.h" #define _USE_MATH_DEFINES 1 #include "math.h" #include "gyro_test.h" #ifdef _DEBUG #define new DEBUG_NEW #endif int iDoubleGyro = 0; char szCalibrateFiles[30][40] ={"Calibrate_28", "Calibrate_27", "Calibrate_26", "Calibrate_25", "Calibrate_24", "Calibrate_23", "Calibrate_22", "Calibrate_21", "Calibrate_20", "Calibrate_19", "Calibrate_18", "Calibrate_17", "Calibrate_16", "Calibrate_15", "Calibrate_14", "Calibrate_13", "Calibrate_12", "Calibrate_11", "Calibrate_10", "Calibrate_9", "Calibrate_8", "Calibrate_7", "Calibrate_6", "Calibrate_5", "Calibrate_4", "Calibrate_3", "Calibrate_2", "Calibrate_1", "Calibrate_0", "Calibrate"}; // The one and only application object CWinApp theApp; using namespace std; int RemoveESC(unsigned char *Buffer, int iLen) { int iScanESC =0; int iSkipESC = 0; for (; iScanESC < iLen; iScanESC++, iSkipESC++) { if (Buffer[iScanESC] == '#') { if (++iScanESC < iLen) ; else break; } Buffer[iSkipESC] = Buffer[iScanESC]; } return iSkipESC; } BOOL WriteFileCom( HANDLE hFile, // handle to file to write to LPCVOID lpBuffer, // pointer to data to write to file DWORD nNumberOfBytesToWrite, // number of bytes to write LPDWORD lpNumberOfBytesWritten, // pointer to number of bytes written LPOVERLAPPED lpOverlapped // pointer to structure for overlapped I/O ) { if (WriteFile(hFile, lpBuffer, nNumberOfBytesToWrite, lpNumberOfBytesWritten, lpOverlapped)) { FlushFileBuffers(hFile ); return TRUE; } else { //for (int iatt=0; iatt<5; iatt++) { DWORD dwErr = GetLastError(); if (PurgeComm( hFile, PURGE_TXCLEAR | PURGE_RXCLEAR )) { if (WriteFile(hFile, lpBuffer, nNumberOfBytesToWrite, lpNumberOfBytesWritten, lpOverlapped)) { FlushFileBuffers(hFile ); return TRUE; } } } } return FALSE; } BOOL ReadFileCom( HANDLE hFile, // handle to file to write to LPCVOID lpBuffer, // pointer to data to write to file DWORD nNumberOfBytesToRead, // number of bytes to write LPDWORD lpNumberOfBytesRead, // pointer to number of bytes written LPOVERLAPPED lpOverlapped // pointer to structure for overlapped I/O ) { if (ReadFile(hFile, (LPVOID)lpBuffer, nNumberOfBytesToRead, lpNumberOfBytesRead, lpOverlapped)) { //FlushFileBuffers(hFile ); return TRUE; } else { //for (int iatt=0; iatt<5; iatt++) { DWORD dwErr = GetLastError(); if (PurgeComm( hFile, PURGE_TXCLEAR | PURGE_RXCLEAR )) { if (ReadFile(hFile, (LPVOID)lpBuffer, nNumberOfBytesToRead, lpNumberOfBytesRead, lpOverlapped)) { //FlushFileBuffers(hFile ); return TRUE; } } } } return FALSE; } static long int iTemCounts[256*256]; static long int XCalibr[256*256]; static long int YCalibr[256*256]; static long int ZCalibr[256*256]; static long int iTCnts[256*256]; static long int XCal[256*256]; static long int YCal[256*256]; static long int ZCal[256*256]; #define COEF_FOR_DEVISION 31 // for C0C0 term reding /*static long int PrevTemp[256*2]; static long int PrevX[256*2]; static long int PrevY[256*2]; static long int PrevZ[256*2]; */ int _tmain(int argc, TCHAR* argv[], TCHAR* envp[]) { int nRetCode = 0; FILE *DatOut = fopen("DatOut.txt","a"); long int XX0 =0; long int YY0 = 0; long int ZZ0 = 0; long long lXX0 =0; long long lYY0 = 0; long long lZZ0 = 0; unsigned char FM8BOUT[40]; int CountWrite; unsigned char FM8B[20]; FM8B[0]=0x31; FM8B[1]=0x46; FM8B[2]=0x01; FM8B[3]=0x06; FM8B[4]=0x46; FM8B[5]= 9 + 3 + 1; // 9 bytes + 3 address + 1 opearion write FM8B[6]=0x02; FM8B[7]=0x00; // address FM8B[8]=0x00; FM8B[9]=0x00; FM8B[10]=0x01; // adj XXX FM8B[11]=0x02; FM8B[12]=0x03; FM8B[13]=0x04; // adj YYY FM8B[14]=0x05; FM8B[15]=0x06; FM8B[16]=0x07; // adj ZZZ FM8B[17]=0x08; FM8B[18]=0x09; FM8B[19]=0x31; unsigned char FM16BOUT[54]; //int CountWrite; unsigned char FM16B[27]; FM16B[0]=0x31; // unit == '1' FM16B[1]=0x46; // flash operation FM16B[2]=0x01; // lenghth == 1 FM16B[3]=0x06; // unblock write FM16B[4]=0x46; // flash operation again FM16B[5]=16+3+1; // length of the pack 20 bytes= 16 data + 3 address + 1 operastion write FM16B[6]=0x02; // operation write FM16B[7]=0x00; // address FM16B[8]=0x00; FM16B[9]=0x00; FM16B[10]=0x01; // byte 0 FM16B[11]=0x02; FM16B[12]=0x03; FM16B[13]=0x04; FM16B[14]=0x05; FM16B[15]=0x06; FM16B[16]=0x07; FM16B[17]=0x08; FM16B[18]=0x09; FM16B[19]=0x0a; FM16B[20]=0x0b; FM16B[21]=0x0c; FM16B[22]=0x0d; FM16B[23]=0x0e; FM16B[24]=0x0f; FM16B[25]=0x10; FM16B[26]=0x31; // end of the packet HMODULE hModule = ::GetModuleHandle(NULL); if (hModule != NULL) { // initialize MFC and print and error on failure if (!AfxWinInit(hModule, NULL, ::GetCommandLine(), 0)) { // TODO: change error code to suit your needs _tprintf(_T("Fatal Error: MFC initialization failed\n")); nRetCode = 1; } else { // TODO: code your application's behavior here. nRetCode = 1; int iHCounter = 0; HANDLE hComHandle; char szSwitchCom[MAX_PATH]= {"com4"}; COMMTIMEOUTS ct; DWORD EvtMask = EV_RXCHAR | EV_ERR; DCB m_dcb; HANDLE hX10ComHandle = NULL; DWORD lenwrt; char MyByte; unsigned char bBuffer[4096]; unsigned char bBuff[4096]; hComHandle = CreateFile(szSwitchCom, GENERIC_READ | GENERIC_WRITE, 0, // exclusive access NULL, // no security attributes OPEN_EXISTING, NULL, //FILE_FLAG_OVERLAPPED, // overlapped I/O NULL); if (INVALID_HANDLE_VALUE != hComHandle) { // set up read/write timeouts GetCommTimeouts( hComHandle, &ct ); ct.ReadIntervalTimeout = MAXDWORD; ct.ReadTotalTimeoutMultiplier = MAXDWORD; ct.ReadTotalTimeoutConstant = 100; ct.WriteTotalTimeoutMultiplier = 5; ct.WriteTotalTimeoutConstant = 1000; SetCommTimeouts( hComHandle, &ct ); // get default parameters m_dcb.DCBlength = sizeof( m_dcb ); if ( GetCommState( hComHandle, &m_dcb ) ) { // parity is configurable, but we only support 8,N,1 m_dcb.BaudRate = CBR_57600;//CBR_38400;//CBR_9600; m_dcb.ByteSize = 8; m_dcb.Parity = NOPARITY; m_dcb.StopBits = ONESTOPBIT; m_dcb.fBinary = TRUE; // binary data m_dcb.fParity = FALSE;//TRUE; // enable parity checking m_dcb.fOutxCtsFlow = FALSE;//TRUE; // cts flow control when sending data m_dcb.fOutxDsrFlow = FALSE; // DSR not required from modem m_dcb.fRtsControl = RTS_CONTROL_DISABLE;//RTS_CONTROL_ENABLE;//RTS_CONTROL_DISABLE;//RTS_CONTROL_HANDSHAKE; // rts flow control when receiving data m_dcb.fDtrControl = DTR_CONTROL_DISABLE;//DTR_CONTROL_ENABLE; // DTR required by modem m_dcb.fDsrSensitivity = FALSE; // DSR not required from modem m_dcb.fOutX = FALSE; // no xon/xoff flow controll m_dcb.fInX = FALSE; m_dcb.fTXContinueOnXoff = FALSE; m_dcb.fErrorChar = FALSE; // don't replace bad chars m_dcb.fNull = FALSE; // don't process null chars m_dcb.fAbortOnError = FALSE;//TRUE; // handle errors m_dcb.ErrorChar= 0xff; // error replacement character SetCommState( hComHandle, &m_dcb ); SetCommMask( hComHandle, EvtMask); EscapeCommFunction( hComHandle,CLRDTR); EscapeCommFunction( hComHandle,SETRTS); memset(bBuffer, 0, sizeof(bBuffer)); //#define DAC_OK "1?1" // WriteFileCom( hComHandle, DAC_OK, sizeof(DAC_OK)-1, &lenwrt, NULL ); // Sleep(100); // ReadFileCom( hComHandle, bBurrrrrrrrrrrrrrrffer, sizeof(bBuffer), &lenwrt, NULL ); // printf("=%s", &bBuffer[0]); printf("\n ? - for help"); while(1) { printf("===>"); MyByte = getchar(); if(MyByte == 'q') { break; } if(MyByte == '?') { printf("\n P - process calibration files - files Calibrate.dat calibrateXX.dat will "); printf("\n be processed and resulting files will be created CalibrateXX_O.dat files"); printf("\n 1 - write stream over I2C to device 8"); printf("\n 2 - read over I2C 1 byte from device 00 with offset 00 (change device and "); printf("\n offset if needed output will be forwarded to com"); printf("\n 3 - read 1 byte from I2C dev 0x68 from offeset 0 => output to com"); printf("\n 4($)- write 1 byte to dev 0x68/0x69, offset 0x3e -> output to com(0x1e is a osc"); printf("\n selection registr)"); printf("\n 5(%)- read 14 bytes (all registers) from device 0x68/0x69 offset 0x15"); printf("\n C(G)- calibrate Gyro 0c86/0x69- temperature swing must apply. output files "); printf("\n CalibrateXX.dat or GalibrateXX.dat(each == 5 minutes) "); printf("\n c - stop calibration collection"); printf("\n 6 - Bradis's table creation"); printf("\n 7 - Bradis's table write to flash"); printf("\n 8 - read FLASH back by 16 bytes read"); printf("\n Q - read Gyro integral values"); printf("\n I(i)- init two GYROs 0x68+0x69 (i) init GYROs to work together "); printf("\n S - start Gyro"); printf("\n P - stop Gyro"); printf("\n F - test Flash memory: send sequence of commands:"); printf("\n F(len=x05)(x03='read')(addr=x00x00x00)(switch to read=@=delimiter)"); printf("\n (read len x04 bytes)"); printf("\n f - test flash memory: sequence of commands:"); printf("\n F(len=x01)(x06='write enable')(x46=F=start)(len=x04)"); printf("\n (x20='sector 4K erase')(addr=x00x00x00)"); printf("\n Z - test flash memory: sequence"); printf("\n F(len=x01)(x06='write enable')(x46=F=start)(len=x0c)"); printf("\n (x02=='program memory')(addr=x00x00x00)(data 8 byte)"); printf("\n X - procees calibration and create FLASH.HEX file"); printf("\n x - procees calibration and create FLASH.HEX file Gyr2=Y-X-Z"); printf("\n E - erase FLASh memory"); printf("\n V - program FLASH memory with calibrated values"); printf("\n v - read back FLASH memory"); printf("\n H - read one magnetometr measure"); printf("\n h - init magnetometr 50"); printf("\n ; - read 50 values from magnetomtr and avarage it"); printf("\n A - read one acselerometr measure"); printf("\n a - init acselerometr"); printf("\n , - read 50 values acselerometr and avarage it"); printf("\n ===>"); //break; } else if ((MyByte == 'X') || (MyByte == 'x')) // process calibration files and creates FLASH memory calibration stream { FILE *DatCalr0 = fopen("Calibrate.dat","rb"); if (DatCalr0 != NULL) { printf("\n process file Calibrate.dat"); for (long i = 0;i < 256*256; i++) { fread(&iTemCounts[i], sizeof(long), 1, DatCalr0); fread(&XCalibr[i], sizeof(long), 1, DatCalr0); fread(&YCalibr[i], sizeof(long), 1, DatCalr0); fread(&ZCalibr[i], sizeof(long), 1, DatCalr0); // eliminate less statistic values if (iTemCounts[i]<256) { iTemCounts[i] = 0; XCalibr[i] = 0; YCalibr[i] = 0; ZCalibr[i] = 0; } } fclose(DatCalr0); // now aproximate midle points int iFlag =0; long iFirst = 0; long iLast = 0; for (long i = 0;i < 256*256; i++) { if (iFlag ==0) { if (iTemCounts[i]>256) { iFlag = 1; iLast = i; if (iFirst != 0) { double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = 1; ii < (iLast - iFirst) ; ii++) { iTemCounts[iFirst+ii] = 1; // this is indication that it is 256 counts XCalibr[iFirst+ii] = (XF1 + ((double)ii) * DXF12)*256.0; YCalibr[iFirst+ii] = (YF1 + ((double)ii) * DYF12)*256.0; ZCalibr[iFirst+ii] = (ZF1 + ((double)ii) * DZF12)*256.0; } } } } else { if (iTemCounts[i] == 0) { iFlag = 0; iFirst = i -1; } } } // now aproximate low value (temp < XX degree int iSuccess = 0; iFirst = 0; iFlag = 0; for (long i = 0;i < 256*256; i++) { if (iFlag ==0) { if (iTemCounts[i]>256) { iFirst = i; iFlag = 1; i+=2*250; // 2 degree } } else { if (iTemCounts[i] > 3) // needs to get real value { iLast = i; double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = 1; ii <= iFirst; ii++) { iTemCounts[iFirst-ii] = 1; // this is indication that it is 256 counts XCalibr[iFirst-ii] = (XF1 - ((double)ii) * DXF12)*256.0; YCalibr[iFirst-ii] = (YF1 - ((double)ii) * DYF12)*256.0; ZCalibr[iFirst-ii] = (ZF1 - ((double)ii) * DZF12)*256.0; } iSuccess = 1; break; } ///otherwise it will continue to next sample } } if (iSuccess) { // now high temp aproximation iSuccess = 0; iFlag = 0; for (long i = 256*256 - 1;i >=0; i--) { if (iFlag ==0) { if (iTemCounts[i]>256) { iLast = i; iFlag = 1; i-=2*250; // 2 degree } } else { if (iTemCounts[i] > 3) // needs to get real value { iFirst = i; double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = iLast+1; ii <256*256; ii++) { iTemCounts[ii] = 1; // this is indication that it is 256 counts XCalibr[ii] = (XF2 + ((double)(ii-iLast)) * DXF12)*256.0; YCalibr[ii] = (YF2 + ((double)(ii-iLast)) * DYF12)*256.0; ZCalibr[ii] = (ZF2 + ((double)(ii-iLast)) * DZF12)*256.0; } iSuccess = 1; break; } ///otherwise it will continue to next sample } } if (iSuccess) { FILE *DatCalO = fopen("Calibrate_O.dat","wb"); if (DatCalO != NULL) { printf("\n file Calibrate_O.dat created"); if (MyByte == 'x') { for (long i = 0;i < 256*256; i++) { iTCnts[i] = iTemCounts[i]; XCal[i] = XCalibr[i]; YCal[i] = YCalibr[i]; ZCal[i] = ZCalibr[i]; } for (long i = 0;i < 256*256-16*16; i+=16) { double XS = 0; double YS = 0; double ZS = 0; double dSum = 0; for (long io = 0; io < 16*16; io+=16) { if (iTCnts[i+io] > 1) { XS+= (double)XCal[i+io];// * (double)iTCnts[i+io]; YS+= (double)YCal[i+io];// * (double)iTCnts[i+io]; ZS+= (double)ZCal[i+io];// * (double)iTCnts[i+io]; dSum +=(double)iTCnts[i+io]; } else { // this is already multiplied on 256 XS+= (double)XCal[i+io]/256.0;// * (double)iTCnts[i+io]; YS+= (double)YCal[i+io]/256.0;// * (double)iTCnts[i+io]; ZS+= (double)ZCal[i+io]/256.0;// * (double)iTCnts[i+io]; dSum +=(double)iTCnts[i+io]; } } XS/=dSum;YS/=dSum;ZS/=dSum; XCalibr[i+ 8*16] = XS*256; YCalibr[i+ 8*16] = YS*256; ZCalibr[i+ 8*16] = ZS*256; iTemCounts[i+ 8*16] = dSum; } for (long i = 0;i < 256*256; i++) { //if (iTemCounts[i]>1) //{ // XCalibr[i] = ((double)XCalibr[i]/(double)iTemCounts[i]) * 256.0; // YCalibr[i] = ((double)YCalibr[i]/(double)iTemCounts[i]) * 256.0; // ZCalibr[i] = ((double)ZCalibr[i]/(double)iTemCounts[i]) * 256.0; //} fwrite(&iTemCounts[i], sizeof(long), 1, DatCalO); fwrite(&XCalibr[i], sizeof(long), 1, DatCalO); fwrite(&YCalibr[i], sizeof(long), 1, DatCalO); fwrite(&ZCalibr[i], sizeof(long), 1, DatCalO); } } else { for (long i = 0;i < 256*256; i++) { if (iTemCounts[i]>1) { XCalibr[i] = ((double)XCalibr[i]/(double)iTemCounts[i]) * 256.0; YCalibr[i] = ((double)YCalibr[i]/(double)iTemCounts[i]) * 256.0; ZCalibr[i] = ((double)ZCalibr[i]/(double)iTemCounts[i]) * 256.0; } fwrite(&iTemCounts[i], sizeof(long), 1, DatCalO); fwrite(&XCalibr[i], sizeof(long), 1, DatCalO); fwrite(&YCalibr[i], sizeof(long), 1, DatCalO); fwrite(&ZCalibr[i], sizeof(long), 1, DatCalO); } } fclose(DatCalO); } FILE *DatCalX = fopen("GYROFLASH.HEX","wb"); if (DatCalX != NULL) { printf("\n file GYROFLASH.HEX created"); for (long i = 0;i < 256*256; i+=16) { unsigned long ui = i; //ui += 0x10000; FM8B[7]=(unsigned char)(0xff&(ui >> 16)); FM8B[8]=(unsigned char)(0xff&(ui >> 8)); FM8B[9]=(unsigned char)(0xff&(ui)); // offsets will be negative from actual value XCalibr[i] = -XCalibr[i]; YCalibr[i] = -YCalibr[i]; //YCalibr[i] =44*256; ZCalibr[i] = -ZCalibr[i]; FM8B[10]=XCalibr[i]&0xff; FM8B[11]=(XCalibr[i]>>8)&0xff; FM8B[12]=(XCalibr[i]>>16)&0xff; FM8B[13]=YCalibr[i]&0xff; FM8B[14]=(YCalibr[i]>>8)&0xff; FM8B[15]=(YCalibr[i]>>16)&0xff; FM8B[16]=ZCalibr[i]&0xff; FM8B[17]=(ZCalibr[i]>>8)&0xff; FM8B[18]=(ZCalibr[i]>>16)&0xff; CountWrite = sizeof(FM8B); memset(&FM8BOUT,0, sizeof(FM8BOUT)); FM8BOUT[0] = FM8B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM8B)-1; ii++) { if ((FM8B[ii] >='1' && FM8B[ii] <= '9') || (FM8B[ii] == '#')) { // needs to insert escape == # FM8BOUT[iii++] = '#'; } FM8BOUT[iii++] = FM8B[ii]; } FM8BOUT[iii++] = FM8B[ii]; fwrite(&FM8BOUT,iii,1, DatCalX); } fclose(DatCalX); } } else printf("\n not enought data for aproximation"); } else printf("\n not enought data for aproximation"); } DatCalr0 = fopen("Galibrate.dat","rb"); if (DatCalr0 != NULL) { printf("\n process file Galibrate.dat (second GYRO)"); for (long i = 0;i < 256*256; i++) { fread(&iTemCounts[i], sizeof(long), 1, DatCalr0); fread(&XCalibr[i], sizeof(long), 1, DatCalr0); fread(&YCalibr[i], sizeof(long), 1, DatCalr0); fread(&ZCalibr[i], sizeof(long), 1, DatCalr0); // eliminate less statistic values if (iTemCounts[i]<256) { iTemCounts[i] = 0; XCalibr[i] = 0; YCalibr[i] = 0; ZCalibr[i] = 0; } } fclose(DatCalr0); // now aproximate midle points int iFlag =0; long iFirst = 0; long iLast = 0; for (long i = 0;i < 256*256; i++) { if (iFlag ==0) { if (iTemCounts[i]>256) { iFlag = 1; iLast = i; if (iFirst != 0) { double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = 1; ii < (iLast - iFirst) ; ii++) { iTemCounts[iFirst+ii] = 1; // this is indication that it is 256 counts XCalibr[iFirst+ii] = (XF1 + ((double)ii) * DXF12)*256.0; YCalibr[iFirst+ii] = (YF1 + ((double)ii) * DYF12)*256.0; ZCalibr[iFirst+ii] = (ZF1 + ((double)ii) * DZF12)*256.0; } } } } else { if (iTemCounts[i] == 0) { iFlag = 0; iFirst = i -1; } } } // now aproximate low value (temp < XX degree int iSuccess = 0; iFirst = 0; iFlag = 0; for (long i = 0;i < 256*256; i++) { if (iFlag ==0) { if (iTemCounts[i]>256) { iFirst = i; iFlag = 1; i+=2*250; // 2 degree } } else { if (iTemCounts[i] > 3) // needs to get real value { iLast = i; double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = 1; ii <= iFirst; ii++) { iTemCounts[iFirst-ii] = 1; // this is indication that it is 256 counts XCalibr[iFirst-ii] = (XF1 - ((double)ii) * DXF12)*256.0; YCalibr[iFirst-ii] = (YF1 - ((double)ii) * DYF12)*256.0; ZCalibr[iFirst-ii] = (ZF1 - ((double)ii) * DZF12)*256.0; } iSuccess = 1; break; } ///otherwise it will continue to next sample } } if (iSuccess) { // now high temp aproximation iSuccess = 0; iFlag = 0; for (long i = 256*256 - 1;i >=0; i--) { if (iFlag ==0) { if (iTemCounts[i]>256) { iLast = i; iFlag = 1; i-=2*250; // 2 degree } } else { if (iTemCounts[i] > 3) // needs to get real value { iFirst = i; double Dt = iLast - iFirst; double XF1 = XCalibr[iFirst]; XF1 /= (double)iTemCounts[iFirst]; double XF2 = XCalibr[iLast]; XF2 /= (double)iTemCounts[iLast]; double DXF12 = (XF2 - XF1)/Dt; double YF1 = YCalibr[iFirst]; YF1 /= (double)iTemCounts[iFirst]; double YF2 = YCalibr[iLast]; YF2 /= (double)iTemCounts[iLast]; double DYF12 = (YF2 - YF1)/Dt; double ZF1 = ZCalibr[iFirst]; ZF1 /= (double)iTemCounts[iFirst]; double ZF2 = ZCalibr[iLast]; ZF2 /= (double)iTemCounts[iLast]; double DZF12 = (ZF2 - ZF1)/Dt; for (long ii = iLast+1; ii <256*256; ii++) { iTemCounts[ii] = 1; // this is indication that it is 256 counts XCalibr[ii] = (XF2 + ((double)(ii-iLast)) * DXF12)*256.0; YCalibr[ii] = (YF2 + ((double)(ii-iLast)) * DYF12)*256.0; ZCalibr[ii] = (ZF2 + ((double)(ii-iLast)) * DZF12)*256.0; } iSuccess = 1; break; } ///otherwise it will continue to next sample } } if (iSuccess) { FILE *DatCalO = fopen("Galibrate_O.dat","wb"); if (DatCalO != NULL) { printf("\n file Galibrate_O.dat created second GYRO"); if (MyByte == 'x') { for (long i = 0;i < 256*256; i++) { iTCnts[i] = iTemCounts[i]; XCal[i] = XCalibr[i]; YCal[i] = YCalibr[i]; ZCal[i] = ZCalibr[i]; } for (long i = 0;i < 256*256-16*16; i+=16) { double XS = 0; double YS = 0; double ZS = 0; double dSum = 0; for (long io = 0; io < 16*16; io+=16) { if (iTCnts[i+io] > 1) { XS+= (double)XCal[i+io];// * (double)iTCnts[i+io]; YS+= (double)YCal[i+io];// * (double)iTCnts[i+io]; ZS+= (double)ZCal[i+io];// * (double)iTCnts[i+io]; dSum +=(double)iTCnts[i+io]; } else { // this is already multiplied on 256 XS+= (double)XCal[i+io]/256.0;// * (double)iTCnts[i+io]; YS+= (double)YCal[i+io]/256.0;// * (double)iTCnts[i+io]; ZS+= (double)ZCal[i+io]/256.0;// * (double)iTCnts[i+io]; dSum +=(double)iTCnts[i+io]; } } XS/=dSum;YS/=dSum;ZS/=dSum; XCalibr[i+ 8*16] = XS*256; YCalibr[i+ 8*16] = YS*256; ZCalibr[i+ 8*16] = ZS*256; iTemCounts[i+ 8*16] = dSum; } for (long i = 0;i < 256*256; i++) { //if (iTemCounts[i]>1) //{ // XCalibr[i] = ((double)XCalibr[i]/(double)iTemCounts[i]) * 256.0; // YCalibr[i] = ((double)YCalibr[i]/(double)iTemCounts[i]) * 256.0; // ZCalibr[i] = ((double)ZCalibr[i]/(double)iTemCounts[i]) * 256.0; //} fwrite(&iTemCounts[i], sizeof(long), 1, DatCalO); fwrite(&XCalibr[i], sizeof(long), 1, DatCalO); fwrite(&YCalibr[i], sizeof(long), 1, DatCalO); fwrite(&ZCalibr[i], sizeof(long), 1, DatCalO); } } else { for (long i = 0;i < 256*256; i++) { if (iTemCounts[i]>1) { XCalibr[i] = ((double)XCalibr[i]/(double)iTemCounts[i]) * 256.0; YCalibr[i] = ((double)YCalibr[i]/(double)iTemCounts[i]) * 256.0; ZCalibr[i] = ((double)ZCalibr[i]/(double)iTemCounts[i]) * 256.0; } fwrite(&iTemCounts[i], sizeof(long), 1, DatCalO); fwrite(&XCalibr[i], sizeof(long), 1, DatCalO); fwrite(&YCalibr[i], sizeof(long), 1, DatCalO); fwrite(&ZCalibr[i], sizeof(long), 1, DatCalO); } fclose(DatCalO); } } FILE *DatCalX = fopen("GYROFLASH.HEX","ab"); if (DatCalX != NULL) { printf("\n file GYROFLASH.HEX appended"); for (long i = 0;i < 256*256; i+=16) { unsigned long ui = i; ui+= 0x10000; FM8B[7]=(unsigned char)(0xff&(ui >> 16)); FM8B[8]=(unsigned char)(0xff&(ui >> 8)); FM8B[9]=(unsigned char)(0xff&(ui)); XCalibr[i] = -XCalibr[i]; YCalibr[i] = -YCalibr[i]; //YCalibr[i] = -297*256; ZCalibr[i] = -ZCalibr[i]; FM8B[10]=XCalibr[i]&0xff; FM8B[11]=(XCalibr[i]>>8)&0xff; FM8B[12]=(XCalibr[i]>>16)&0xff; FM8B[13]=YCalibr[i]&0xff; FM8B[14]=(YCalibr[i]>>8)&0xff; FM8B[15]=(YCalibr[i]>>16)&0xff; FM8B[16]=ZCalibr[i]&0xff; FM8B[17]=(ZCalibr[i]>>8)&0xff; FM8B[18]=(ZCalibr[i]>>16)&0xff; CountWrite = sizeof(FM8B); memset(&FM8BOUT,0, sizeof(FM8BOUT)); FM8BOUT[0] = FM8B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM8B)-1; ii++) { if ((FM8B[ii] >='1' && FM8B[ii] <= '9') || (FM8B[ii] == '#')) { // needs to insert escape == # FM8BOUT[iii++] = '#'; } FM8BOUT[iii++] = FM8B[ii]; } FM8BOUT[iii++] = FM8B[ii]; fwrite(&FM8BOUT,iii,1, DatCalX); } fclose(DatCalX); } } else printf("\n not enought data for aproximation for second GYRO"); } else printf("\n not enought data for aproximation for GYRO2"); } } else if(MyByte == '1') { #define INIT_DAC_ADC "2221<8qwertyuioqwertyuioqwertyuioqwertyuioabc@1222" WriteFileCom( hComHandle, INIT_DAC_ADC, sizeof(INIT_DAC_ADC)-1, &lenwrt, NULL ); memset(bBuffer, 0, sizeof(bBuffer)); Sleep(2000); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == '2') { #define INIT_GYRO_RESPONCE "2221=5`c<\x00\x00>\x01@1222" for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_RESPONCE, sizeof(INIT_GYRO_RESPONCE)-1); if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_RESPONCE)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); if (bBuffer[5] != 0xff) printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == '3') { //#define INIT_GYRO_1 "1=5`C<\x68#\x15@1 1=5`C>\x68\x0e\0x31" //#define INIT_GYRO_1 "1=5`C<\x68#\x15@1" #define INIT_GYRO_1 "1=5`C>\x68\x0e@1" //#define INIT_GYRO_1 "1=5`C<\x68#\x15@1 1=5`C<\x68#\x15@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_1, sizeof(INIT_GYRO_1)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_1)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if((MyByte == '4') || (MyByte == '$')) // use specific oscilator for GYRO 1 { //#define INIT_GYRO_2 "2221=5`C<\x68\x16\x18@<\x68\x16>\x02@<\x68#\x3e\x02@<\x68#\x3e>\x01@1222" // Gyro Z as a reference: #define INIT_GYRO_2 "1=5`C<\x68#\x3e\x03@<\x68#\x3e>\x01@1" // Gyro Y as a reference: //#define INIT_GYRO_2 "1=5`C<\x68#\x3e\x02@<\x68#\x3e>\x01@1" // Gyro X as a reference: //#define INIT_GYRO_2 "1=5`C<\x68#\x3e\x01@<\x68#\x3e>\x01@1" // Gyro internal osc: //#define INIT_GYRO_2 "1=5`C<\x68#\x3e\x00@<\x68#\x3e>\x01@1" // reset Gyro //#define INIT_GYRO_RESET "1=5`C<\x68#\x3e\x80@<\x68#\x3e>\x01@1reset gyro" #define INIT_GYRO_RESET "1=5`C<\x68#\x3e\x80@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_RESET, sizeof(INIT_GYRO_RESET)-1); if (MyByte == '4') bBuffer[6] = 0x68; else bBuffer[6] = 0x69; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_RESET)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); memset(bBuffer, 0, sizeof(bBuffer)); memcpy(bBuffer,INIT_GYRO_2, sizeof(INIT_GYRO_2)-1); if (MyByte == '4') bBuffer[6] = 0x68; else bBuffer[6] = 0x69; //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_2)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if((MyByte == '5') || (MyByte == '%')) { //#define INIT_GYRO_3 "2221=5`c<\x68#\x1a>\x09@<\x68\x1d>\x02@<\x68\x1f>\x02@<\x68\x21>\x02@<\x68\x1a>\x01@1222" #define INIT_GYRO_33 "1=5`C<\x68#\x15>\x0e@1" //#define INIT_GYRO_33 "1=5`C<\x68#\x1a>\x0e@1" //#define INIT_GYRO_33 "234567890234567890234567890234567890234567890" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_33, sizeof(INIT_GYRO_33)-1); if (MyByte == '5') bBuffer[6] = 0x68; else bBuffer[6] = 0x69; //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_33)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if((MyByte == 'C') || (MyByte == 'G')) /// calibrate gyro 0x68/0x69- temperature swing must apply { unsigned short iBeginTemp = 0; char szFileName[256]; FILE *DatCalr0 = NULL;// if (MyByte == 'C') DatCalr0 = fopen("Calibrate.dat","rb"); else DatCalr0 = fopen("Galibrate.dat","rb"); if (DatCalr0 != NULL) { for (long i = 0;i < 256*256; i++) { fread(&iTemCounts[i], sizeof(long), 1, DatCalr0); //if (iTemCounts[i]) // printf("%ld ", iTemCounts[i]); fread(&XCalibr[i], sizeof(long), 1, DatCalr0); fread(&YCalibr[i], sizeof(long), 1, DatCalr0); fread(&ZCalibr[i], sizeof(long), 1, DatCalr0); if ((i >=0xcd60) && (ZCalibr[0xcd60] == 0)) ZCalibr[0xcd60] = ZCalibr[0xcd60]; } fclose(DatCalr0); } else { for (long i = 0;i < 256*256; i++) { iTemCounts[i] = 0; XCalibr[i] = 0; YCalibr[i] = 0; ZCalibr[i] = 0; } } /*{ for (long i = 0;i < 2*256; i++) { PrevTemp[i] = 0; PrevX[i] = 0; PrevY[i] = 0; PrevZ[i] = 0; } } if (ZCalibr[0xcd60] == 0) ZCalibr[0xcd60] = ZCalibr[0xcd60]; */ #define INIT_GYRO_C "1C1" for (int iAttempts = 0; iAttempts < 1; iAttempts++) { int iFirstOUT = 0; memcpy(bBuffer,INIT_GYRO_C, sizeof(INIT_GYRO_C)-1); if (MyByte == 'C') bBuffer[1] = 'C'; else bBuffer[1] = 'G'; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_C)-1, &lenwrt, NULL ); printf("\n lets memory reset"); Sleep(10000); //printf("\n lets gyro idle 4 seconds"); //Sleep(4000); unsigned long dwDelay = 18000; dwDelay = dwDelay*COEF_FOR_DEVISION/7; printf("\n lets gyro run idle %d seconds",dwDelay/1000); Sleep(dwDelay); //Sleep(2000); memset(bBuffer, 0, sizeof(bBuffer)); int iErrorFlash = 0; int WriteLen = 0; int OldTemp = 0; char TypeP[2]; if (MyByte == 'C') TypeP[0] = 'c'; else TypeP[0] = 'g'; for (long int iReads = 0; iReads < 2048*8; iReads+=16) { unsigned long ui = (unsigned long)iReads; ui *=8; ui +=0x30000; #define FLASH_32_READ "1=5 F\x05\x03\x00\x00\x00@\x80\x31" memset(bBuff,0,sizeof(bBuff)); memcpy(bBuff,FLASH_32_READ, sizeof(FLASH_32_READ)-1); bBuff[8]=(unsigned char)(0xff&(ui >> 16)); bBuff[9]=(unsigned char)(0xff&(ui >> 8)); bBuff[10]=(unsigned char)(0xff&(ui)); bBuff[100] = bBuff[0]; bBuff[101] = bBuff[1]; bBuff[102] = bBuff[2]; bBuff[103] = bBuff[3]; bBuff[104] = bBuff[4]; int ii = 5; int iii = 5; for (ii = 5; ii < sizeof(FLASH_32_READ)-1-1; ii++) { if ((bBuff[ii] >='1' && bBuff[ii] <= '9') || (bBuff[ii] == '#')) { // needs to insert escape == # bBuff[100+iii] = '#'; iii++; } bBuff[100+iii] = bBuff[ii]; iii++; } bBuff[100+iii] = bBuff[ii]; iii++; WriteLen = iii; WriteFileCom( hComHandle, &bBuff[100], WriteLen, &lenwrt, NULL ); //Sleep(100); iii = 0; ii = 0; memset(bBuffer, 0, sizeof(bBuffer)); int iLooping =0; Sleep(40); while(iii<256) // esc char can double message { ReadFileCom( hComHandle, &bBuffer[iii], sizeof(bBuffer), &lenwrt, NULL ); iii+= lenwrt; if (iii > 128 && bBuffer[iii-1] == '5') // indication the end of package break; if (++iLooping < 10) continue; Sleep(100); if (++iLooping < 12) continue; break; } lenwrt = iii; lenwrt= RemoveESC(bBuffer, lenwrt); if (iii < 130) { printf("len!!"); } if (bBuffer[0] == '5' && bBuffer[129] == '5') { } else { printf("\npkt wrng="); for (int ip = 0; ip < lenwrt ; ip++) { printf("%02x ",bBuffer[ip]); } continue; } iErrorFlash = 0; unsigned int Itemp; unsigned short int ItempX; unsigned short int ItempY; unsigned short int ItempZ; short int *pItemp; short int *pItempX; short int *pItempY; short int *pItempZ; for(int iN = 0; iN<128; iN+=8) { Itemp = 0; ItempX = 0; ItempY = 0; ItempZ = 0; Itemp = bBuffer[iN+1]; Itemp <<=8; Itemp += bBuffer[iN+2]; ItempX = bBuffer[iN+3]; ItempX <<=8; ItempX += bBuffer[iN+4]; ItempY = bBuffer[iN+5]; ItempY <<=8; ItempY += bBuffer[iN+6]; ItempZ = bBuffer[iN+7]; ItempZ <<=8; ItempZ += bBuffer[iN+8]; pItemp = (short int*)&Itemp; pItempX = (short int*)&ItempX; pItempY = (short int*)&ItempY; pItempZ = (short int*)&ItempZ; if ((Itemp & 0x0f) != 0) { printf("\nsomething wrong with Temp =%x ",Itemp); for (int ip = 0; ip < 10 ; ip++) { printf("%02x ",bBuffer[iN+ip]); } } else { //if (iTemCounts[Itemp] >= 256*16) //{ // iTemCounts[Itemp]= iTemCounts[Itemp]/2; // XCalibr[Itemp] = XCalibr[Itemp]/2; // YCalibr[Itemp] = YCalibr[Itemp]/2; // ZCalibr[Itemp] = ZCalibr[Itemp]/2; //} if (OldTemp!= 0)// && ((Itemp == OldTemp))) { iTemCounts[Itemp]++; XCalibr[Itemp] += (long)(*pItempX); YCalibr[Itemp] += (long)(*pItempY); ZCalibr[Itemp] += (long)(*pItempZ); //iTemCounts[(Itemp+OldTemp)/2]++; //XCalibr[(Itemp+OldTemp)/2] += (long)(*pItempX); //YCalibr[(Itemp+OldTemp)/2] += (long)(*pItempY); //ZCalibr[(Itemp+OldTemp)/2] += (long)(*pItempZ); iFirstOUT = iReads % 2048; if ((iFirstOUT ==0) && (iN==0)) { iFirstOUT = 1; printf("\n Temp =%x X=%04x Y=%04x Z=%04x",Itemp,ItempX,ItempY,ItempZ); } { char szName1[128]; sprintf(szName1,"%c_%04x",TypeP[0],Itemp); FILE *FileTemp=fopen(szName1,"a"); if (FileTemp) { fprintf(FileTemp,"x=%d y=%d z=%d, count=%d, old temp =%04x\n", (long)(*pItempX),(long)(*pItempY),(long)(*pItempZ),iN/8,OldTemp); fclose(FileTemp); } } } OldTemp = Itemp; } } } } if (MyByte == 'C') { MoveFile("Calibrate.dat","Calibrate_0.dat"); MoveFile("Calibrate_O.dat","Calibrate_0_O.dat"); } else { MoveFile("galibrate.dat","Galibrate_0.dat"); MoveFile("Galibrate_O.dat","Galibrate_0_O.dat"); } FILE *DatCal = NULL; FILE *DatCalO = NULL; if (MyByte == 'C') { DatCal = fopen("Calibrate.dat","wb"); DatCalO = fopen("Calibrate_O.dat","wb"); } else { DatCal = fopen("Galibrate.dat","wb"); DatCalO = fopen("Galibrate_O.dat","wb"); } if (DatCal != NULL) { for (long i = 0;i < 256*256; i++) { if ((ZCalibr[i] == 0) && (iTemCounts[i] != 0)) ZCalibr[i] = ZCalibr[i]; fwrite(&iTemCounts[i], sizeof(long), 1, DatCal); fwrite(&XCalibr[i], sizeof(long), 1, DatCal); fwrite(&YCalibr[i], sizeof(long), 1, DatCal); fwrite(&ZCalibr[i], sizeof(long), 1, DatCal); if (iTemCounts[i]) { if (iTemCounts[i] < 6)//16)//256) { XCalibr[i] = 0; YCalibr[i] = 0; ZCalibr[i] = 0; } else { XCalibr[i] = ((double)XCalibr[i]/(double)iTemCounts[i]) * 256.0; YCalibr[i] = ((double)YCalibr[i]/(double)iTemCounts[i]) * 256.0; ZCalibr[i] = ((double)ZCalibr[i]/(double)iTemCounts[i]) * 256.0; } } //fwrite(&i, sizeof(long), 1, DatCalO); fwrite(&iTemCounts[i], sizeof(long), 1, DatCalO); fwrite(&XCalibr[i], sizeof(long), 1, DatCalO); fwrite(&YCalibr[i], sizeof(long), 1, DatCalO); fwrite(&ZCalibr[i], sizeof(long), 1, DatCalO); } fclose(DatCal); fclose(DatCalO); } } else if(MyByte == 'c') /// stop calibrate gyro - temperature swing must apply { #define INIT_GYRO_C_STOP "1c1" memcpy(bBuffer,INIT_GYRO_C_STOP, sizeof(INIT_GYRO_C_STOP)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_C_STOP)-1, &lenwrt, NULL ); Sleep(1000); do { memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); } while (lenwrt); printf("Calibr done===>"); } else if(MyByte == '6') { #ifdef OLD_BRADIS double MyDeg; double MyDegDelta = 0.00005; double MySq; double MyMeasure; double MyCos_2; double MySin_2_Q; unsigned long dwMyCos; unsigned long dwMySin; unsigned long long qwMySQ; unsigned long dwExtract; unsigned long dwExtractOld = 0; unsigned int iShift = 20; unsigned long dwAddress = 0x04fffe; unsigned long dwAddressOld = 0x04fffe; unsigned long dwOldCompare = 0; unsigned int iAdd = 0; unsigned int iAddSin = 0; int iAddDiff = 0; unsigned int iAddSinOld = 0; int iAddDiffOld = 0; int iDoneOnec = 0; int i2or4 = 2; int swt5_4 = 6; long DiffSinCos; unsigned long NextShift = 0x10000; unsigned char bFLASH[128]; int iPointerFLASH = 0; unsigned long dwFLASHADDR; int iCounBytes = 0; int FirstEntryBig = 0; int FirstEntry1000 = 0; FILE *FOut1 = fopen("MyDeg.txt","w"); FILE *FOutH = fopen("MyDeg.h","w"); FILE *FOutF = fopen("MyDeg.flash","w"); FILE *FOutC = fopen("MyDeg.c","w"); FILE *FOutf = fopen("MyDeg.HEX","wb"); fprintf(FOutF,"\noff= 0x%06x cos=%02x sin=%02x // val =%06x", dwAddress, (unsigned long)0,(unsigned long)0,(unsigned long)0); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift, bigger and equal then ", 0,0); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", 0,0,0,dwAddress); fprintf(FOutC,"\n OFFSET_SIN= 0; OFFSET_COS= 0; // initial clean"); fprintf(FOutC,"\n DW_MIDDLE>>=4; // inital shift = 20 bits"); dwAddressOld = dwAddress; for (MyDeg=0;MyDeg<142;MyDeg+=MyDegDelta)//0.0005) { MyMeasure = MyDeg *230*256; MySq = MyMeasure*MyMeasure; qwMySQ = MySq; dwExtract = qwMySQ>>20; // >> 16 //dwExtract &=0xfffffff0; MyCos_2 = cos(MyDeg*M_PI/180/2.0); dwMyCos = (1 - MyCos_2)*256.*256.*256.; MySin_2_Q = sin(MyDeg*M_PI/180/2.0) / (MyDeg*M_PI/180); dwMySin = (0.5-MySin_2_Q)*256.0*256.0*256.0; DiffSinCos = dwMySin*swt5_4-dwMyCos; //if (abs(((long)dwMySin*swt5_4-(long)dwMyCos) < 0x800)) // ; //else // swt5_4--; //fprintf(FOut1,"\n %f %f %08x %08x %08x %08x %08x %d",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin, dwMySin*swt5_4-dwMyCos,swt5_4); if (dwExtract >= 0x3ff940000) // case 141 { } else if (dwExtract >= 0x07d65300)// case bigger 45 dgree { } else if (dwExtract > 0x0000fff0)//(dwExtract > 0x00001f0d)//(dwExtract > 0x00010000)//(dwExtract > 0x000052CC)// case 0.633 - 1.113000 two byte difference { int iAddSinOldS,iAddSinS,iAddDiffS,iAddDiffOldS; int IFlagShowShift = 0; if ((dwExtractOld>>(iShift-20)) != (dwExtract>>(iShift-20))) { if (((dwExtract - dwExtractOld)>>(iShift-20)) > 1) fprintf(FOutF,"\nDifference from %x to %x bigger then 1",dwExtract,dwExtractOld); if (FirstEntryBig == 0) { FirstEntryBig = 1; iAddSinOldS=iAddSinOld;iAddSinS=iAddSin;iAddDiffS=iAddDiff;iAddDiffOldS=iAddDiffOld; iAddSinOld = iAddSin = iAddDiff = iAddDiffOld = 0; dwAddress+=i2or4; // 2 bytes fprintf(FOutC,"\n if (DW_MIDDLE < 0x%04x) //angle less then %f",(dwExtract>>(iShift-20)),MyDeg); fprintf(FOutC,"\n {"); fprintf(FOutC,"\n OFFSET_SIN_SMALL= 0x%02x; OFFSET_COS= 0x%02x;",iAddSinOldS+1,iAddDiffOldS); i2or4 = 4; if ((dwExtract>>(iShift-20) ==0x1000)) fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE)<<1);",dwAddressOld); else fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE - 0x%04x)<<1);",dwAddressOld,dwOldCompare); fprintf(FOutC,"\n PROC_2_BYTES();"); fprintf(FOutC,"\n return;"); fprintf(FOutC,"\n }"); fprintf(FOutF,"\n next mask=%08x shft=%d", NextShift,iShift); iDoneOnec = 1; dwAddressOld = dwAddress; fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-20,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwAddress-=4; // 4 bytes dwOldCompare = (dwExtract>>(iShift-20)); } if (abs((DiffSinCos)-(long)iAddDiff*0x0010000) >=0x10000) { iAddDiffOld = iAddDiff; iAddDiff++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x sin=%04x Dif=%04x(%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x10000, ((unsigned long)dwMySin)-iAddSin*0x10000, DiffSinCos-(long)iAddDiff*0x10000, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); } if ((((unsigned long)dwMySin)-iAddSin*0x10000) >=0x10000) { iAddSinOld = iAddSin; iAddSin++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x Sin=%04x (%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x10000, ((unsigned long)dwMySin)-iAddSin*0x10000,(unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); } //if ((((unsigned long)dwMyCos)-iAdd*0x10000) >=0x10000) //{ // iAdd++; // iDoneOnec = 0; // fprintf(FOutF,"\n//Cos=%04x sin=%04x (%08x %08x %f)", // ((unsigned long)dwMyCos)-iAdd *0x10000, // ((unsigned long)dwMySin)-iAddSin*0x10000,(unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); //} if (/*(MyDeg < 70.0) &&*/ ((NextShift & dwExtract)!=0) ) { fprintf(FOutF,"\n//cos=%04x sin=%04x*(%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd*0x10000,(unsigned long)dwMySin,(unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); NextShift <<=4; //if (FirstEntry1000) // fprintf(FOutC,"\n DW_MIDDLE>>=4;"); IFlagShowShift = 1; iDoneOnec = 0; iShift+=4; fprintf(FOutF,"next mask=%08x shft=%d", NextShift,iShift); } if (iDoneOnec == 0) { dwAddress+=i2or4; // 4 bytes if (FirstEntry1000) fprintf(FOutC,"\n if (DW_MIDDLE < 0x%04x) //angle less then %f",(dwExtract>>(iShift-20)),MyDeg); else { fprintf(FOutC,"\n if (DW_MIDDLE < 0x%04x) //angle less then %f",(dwExtract>>(iShift-24)),MyDeg); } fprintf(FOutC,"\n {"); fprintf(FOutC,"\n OFFSET_SIN= 0x%02x; OFFSET_COS= 0x%02x;",iAddSinOld,iAddDiffOld); if ((dwExtract>>(iShift-20) ==0x1000)) { if (FirstEntry1000) fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE)<<2);",dwAddressOld); else fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE - 0x%04x)<<2);",dwAddressOld,dwOldCompare); } else fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE - 0x%04x)<<2);",dwAddressOld,dwOldCompare); fprintf(FOutC,"\n PROC_4_BYTES();"); fprintf(FOutC,"\n return;"); fprintf(FOutC,"\n }"); if (FirstEntry1000) { if (IFlagShowShift) fprintf(FOutC,"\n DW_MIDDLE>>=4;"); IFlagShowShift = 0; } else { FirstEntry1000 = 1; //fprintf(FOutC,"\n DW_MIDDLE>>=4;"); } iDoneOnec = 1; dwAddressOld = dwAddress; fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-20,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwAddress-=4; // 4 bytes dwOldCompare = (dwExtract>>(iShift-20)); } dwAddress+=4; // 4 bytes fprintf(FOutF,"\noff= 0x%06x cos=%04x sin=%04x diff=%04x// val =%08x (%08x %08x %f)", dwAddress, ((unsigned long)dwMyCos)-iAdd *0x10000, ((unsigned long)dwMySin)-iAddSin*0x10000, (unsigned int)((DiffSinCos-(long)iAddDiff*0x10000)&0x0000ffff), dwExtract>>(iShift-20), (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); if ((iCounBytes == 16) || (iCounBytes == 14))// 16 bytes to write { FM16B[7]=(unsigned char)(0xff&(dwFLASHADDR >> 16)); FM16B[8]=(unsigned char)(0xff&(dwFLASHADDR >> 8)); FM16B[9]=(unsigned char)(0xff&(dwFLASHADDR)); FM16B[5]=16+3+1; FM16B[5]-=(16-iCounBytes); memcpy(&FM16B[10],bFLASH,iCounBytes); CountWrite = sizeof(FM16B); memset(&FM16BOUT,0, sizeof(FM16BOUT)); FM16BOUT[0] = FM16B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM16B)-1-(16-iCounBytes); ii++) { if ((FM16B[ii] >='1' && FM16B[ii] <= '9') || (FM16B[ii] == '#')) { // needs to insert escape == # FM16BOUT[iii++] = '#'; } FM16BOUT[iii++] = FM16B[ii]; } FM16BOUT[iii++] = FM16B[26]; fwrite(&FM16BOUT,iii,1, FOutf); iCounBytes = 0; } if (iCounBytes == 0) { memset(bFLASH,0,sizeof(bFLASH)); dwFLASHADDR = dwAddress; } bFLASH[iCounBytes++] = (dwMySin)&0x00ff; bFLASH[iCounBytes++] = (dwMySin)>>8; bFLASH[iCounBytes++] = (DiffSinCos)&0x00ff; bFLASH[iCounBytes++] = (DiffSinCos)>>8; } } else // case 0 -0.633 // one byte difference /////////////////////////////////////////////////// { if (dwExtractOld != dwExtract) { if (abs((DiffSinCos)-(long)iAddDiff*0x000100) >=0x100) { iAddDiffOld = iAddDiff; iAddDiff++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x sin=%04x Dif=%04x(%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x10000, ((unsigned long)dwMySin)-iAddSin*0x10000, DiffSinCos-(long)iAddDiff*0x10000, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); } if ((((unsigned long)dwMySin)-iAddSin*0x100) >=0x100) { iAddSinOld = iAddSin; iAddSin++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x Sin=%04x (%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x100, ((unsigned long)dwMySin)-iAddSin*0x100,(unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); } if (iDoneOnec == 0) { iDoneOnec = 1; if (dwExtract >1) { dwAddress+=i2or4; fprintf(FOutC,"\n if (DW_MIDDLE < 0x%04x) //angle less then %f",(dwExtract>>(iShift-20)),MyDeg); fprintf(FOutC,"\n {"); fprintf(FOutC,"\n OFFSET_SIN_SMALL= 0x%02x;",iAddSinOld); if (dwOldCompare == 0) fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE)<<1);",dwAddressOld); else fprintf(FOutC,"\n FLASH_ADDR = 0x%06x + ((DW_MIDDLE - 0x%04x)<<1);",dwAddressOld,dwOldCompare); fprintf(FOutC,"\n PROC_2_BYTES();"); fprintf(FOutC,"\n return;"); fprintf(FOutC,"\n }"); dwAddressOld = dwAddress; fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-20,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwAddress-=i2or4; // 4 bytes dwOldCompare = (dwExtract>>(iShift-20)); } else { fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-20,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwOldCompare = (dwExtract>>(iShift-20)); dwAddressOld +=2; } } dwAddress+=2; // two bytes if (dwExtract - dwExtractOld > 0x1) fprintf(FOutF,"\ndifference from %x to %x bigger then 1",dwExtract,dwExtractOld); fprintf(FOutF,"\noff= 0x%06x cos=%02x sin=%02x diff=%02x// val =%06x (%08x %08x %f)", dwAddress, (unsigned long)dwMyCos, (unsigned long)dwMySin, (unsigned char)(DiffSinCos&0xff), dwExtract>>(iShift-20), (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); if (iCounBytes == 16) // 16 bytes to write { FM16B[7]=(unsigned char)(0xff&(dwFLASHADDR >> 16)); FM16B[8]=(unsigned char)(0xff&(dwFLASHADDR >> 8)); FM16B[9]=(unsigned char)(0xff&(dwFLASHADDR)); memcpy(&FM16B[10],bFLASH,16); CountWrite = sizeof(FM16B); memset(&FM16BOUT,0, sizeof(FM16BOUT)); FM16BOUT[0] = FM16B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM16B)-1; ii++) { if ((FM16B[ii] >='1' && FM16B[ii] <= '9') || (FM16B[ii] == '#')) { // needs to insert escape == # FM16BOUT[iii++] = '#'; } FM16BOUT[iii++] = FM16B[ii]; } FM16BOUT[iii++] = FM16B[ii]; fwrite(&FM16BOUT,iii,1, FOutf); iCounBytes = 0; } if (iCounBytes == 0) { memset(bFLASH,0,sizeof(bFLASH)); dwFLASHADDR = dwAddress; } bFLASH[iCounBytes++] = dwMySin; bFLASH[iCounBytes++] = DiffSinCos; } } dwExtractOld = dwExtract; if (MyDeg >3.00) MyDegDelta = 0.00001; if (MyDeg >3.039) MyDegDelta = 0.000001; if (MyDeg >5.0) MyDegDelta = 0.00001; } fclose(FOutf); fclose(FOutC); fclose(FOut1); fclose(FOutH); fclose(FOutF); #else double MyDeg; double MyDegDelta = 0.00005; double MySq; double MyMeasure; double MyCos_2; double MySin_2_Q; unsigned long long dwMyCos; unsigned long long dwMySin; unsigned long long qwMySQ; unsigned long dwExtract; unsigned long dwExtractOld = 0xffffffff; unsigned int iShift = 28; unsigned long dwAddress = 0x050000; unsigned long dwAddressOld = 0x050000; unsigned long dwOldCompare = 0; unsigned int iAdd = 0; unsigned int iAddSin = 0; int iAddDiff = 0; unsigned int iAddSinOld = 0; int iAddDiffOld = 0; int iDoneOnec = 1; int i2or4 = 2; int swt5_4 = 6; long DiffSinCos; unsigned long NextShift = 0x10000; unsigned char bFLASH[128]; int iPointerFLASH = 0; unsigned long dwFLASHADDR; int iCounBytes = 0; int FirstEntryBig = 0; int FirstEntry1000 = 0; long SamplesPerSec = 250; FILE *FOut1 = fopen("MyDeg.txt","w"); FILE *FOutH = fopen("MyDeg.h","w"); FILE *FOutF = fopen("MyDeg.flash","w"); FILE *FOutC = fopen("MyDeg.c","w"); FILE *FOutf = fopen("MyDeg.HEX","wb"); fprintf(FOutF,"\noff= 0x%06x cos=%02x sin=%02x // val =%06x", dwAddress, (unsigned long)0,(unsigned long)0,(unsigned long)0); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift, bigger and equal then ", 0,0); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", 0,0,0,dwAddress); fprintf(FOutC,"\n// SamplesPerSec =%d;",SamplesPerSec); fprintf(FOutC,"\n OffsetCos.lVal =0; OffsetSin.lVal = 0; // initial clean"); fprintf(FOutC,"\n Square.dwMiddle>>=4; // inital shift = %d bits",iShift); fprintf(FOutC,"\n FlashAddr.dwVal = 0x%05x; // offset in FLASH",dwAddress); dwAddressOld = dwAddress; for (qwMySQ=0;qwMySQ<0x0000ffffffffffff;qwMySQ+=0x10000)//0.0005) { //MyMeasure = MyDeg *230*256; //MySq = MyMeasure*MyMeasure; //qwMySQ = MySq; MySq = qwMySQ; dwExtract = qwMySQ>>28; // >> 16 if (dwExtractOld != dwExtract) { MyMeasure = MySq / ((float)SamplesPerSec*(float)SamplesPerSec); // 250*250 MyMeasure = sqrt(MyMeasure); MyDeg = MyMeasure/230./256.; if (MyMeasure >= (256.*128.)) break; //MyCos_2 = cos(MyDeg*M_PI/180/2.0); //dwMyCos = (1 - MyCos_2)*256.*256.*256.; //MySin_2_Q = sin(MyDeg*M_PI/180/2.0) / (MyDeg*M_PI/180); //dwMySin = (0.5-MySin_2_Q)*256.0*256.0*256.0; //DiffSinCos = dwMySin*swt5_4-dwMyCos; MyCos_2 = cos(MyDeg*M_PI/180.0/2.0); dwMyCos = (1 - MyCos_2)*256.*256.*256.*256.0; // range for a cos is from 0 to 1.0 == from 0x00000000 to MSB=1+0x00000000 if (MyDeg) MySin_2_Q = sin(MyDeg*M_PI/180/2.0) / (MyDeg*M_PI/180); else MySin_2_Q = 0.5; dwMySin = (0.5-MySin_2_Q)*256.0*256.0*256.0*256; // range from 0.5 to 0 but needs to make it 256 less this will even with a cos during multiplication inside pic DiffSinCos = dwMySin*swt5_4-dwMyCos; // this will be constant for calculation // Sin(a/2) /q has to be 256 time less to properly propagate shift //dwMySin /=256; // X Y Z will be 256 scale more the Q as a result multiplication // Q*Q will requare shift 256*256 // Q*X will be 256*256 as a result will requare shift by 32 bits // Qn = + Q1*Q2 - X1*X2 - Y1*Y2 - Z1*Z2 => 0111 // Xn = + X1*Q2 + Q1*X2 - Z1*Y2 + Y1*Z2 => 0010 // Yn = + Y1*Q2 + Z1*X2 + Q1*Y2 - X1*Z2 => 0001 // Zn = + Z1*Q2 - Y1*X2 + X1*Y2 + Q1*Z2 fprintf(FOut1,"\n %f %f %08x %08x %08x %08x %08x ",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin, DiffSinCos);//dwMySin*swt5_4-dwMyCos); if (abs((DiffSinCos)-(long)iAddDiff*0x000100) >=0x100) { iAddDiffOld = iAddDiff; iAddDiff++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x sin=%04x Dif=%04x(%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x10000, ((unsigned long)dwMySin)-iAddSin*0x10000, DiffSinCos-(long)iAddDiff*0x10000, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); //fprintf(FOutC,"\n if (Square.dwMiddle < 0x%05x) //angle less then %f",(dwExtract>>(iShift-28)),MyDeg); //fprintf(FOutC,"\n {"); //fprintf(FOutC,"\n FlashAddr.dwVal = ((Square.dwMiddle)<<1);"); //fprintf(FOutC,"\n PROC_2_BYTES();"); //fprintf(FOutC,"\n return;"); //fprintf(FOutC,"\n }"); //fprintf(FOutC,"\n OffsetSin.bValH++;"); } if ((((unsigned long)dwMySin)-iAddSin*0x100) >=0x100) { iAddSinOld = iAddSin; iAddSin++; iDoneOnec = 0; fprintf(FOutF,"\n//cos=%04x Sin=%04x (%08x %08x %f)", ((unsigned long)dwMyCos)-iAdd *0x100, ((unsigned long)dwMySin)-iAddSin*0x100,(unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); fprintf(FOutC,"\n if (Square.dwMiddle < 0x%05x) //angle less then %f x %d=%f",(dwExtract>>(iShift-28)),MyDeg,SamplesPerSec,MyDeg*SamplesPerSec); fprintf(FOutC,"\n goto END_OF_TABLE;"); //fprintf(FOutC,"\n {"); //fprintf(FOutC,"\n FlashAddr.dwVal += ((Square.dwMiddle)<<1);"); //fprintf(FOutC,"\n PROC_2_BYTES();"); //fprintf(FOutC,"\n return;"); //fprintf(FOutC,"\n }"); fprintf(FOutC,"\n OffsetSin.bValH++;"); } if (iDoneOnec == 0) { iDoneOnec = 1; dwAddressOld = dwAddress; fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-28,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwOldCompare = (dwExtract>>(iShift-28)); } else { fprintf(FOutH,"\n\n// %f %f %08x %08x %08x %08x",MyDeg, MyMeasure, (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,(unsigned long)dwMyCos,(unsigned long)dwMySin); fprintf(FOutH,"\n 0x%02x,0x%06x, // shift,bigger and equal then ", iShift-28,dwExtract);//(dwExtract>>(iShift-16))); fprintf(FOutH,"\n 0x%02x, 0x%02x, 0x%02x, 0x%06x, // address ", iAdd,iAddSin,iAddDiff,dwAddress); dwOldCompare = (dwExtract>>(iShift-20)); dwAddressOld +=2; } fprintf(FOutF,"\noff= 0x%06x cos=%02x sin=%02x diff=%02x// val =%06x (%08x %08x %f)", dwAddress, (unsigned long)dwMyCos, (unsigned long)dwMySin, (unsigned char)(DiffSinCos&0xff), dwExtract>>(iShift-28), (unsigned long)(MySq/256./256./256./256.),(unsigned long)MySq,MyDeg); if (iCounBytes == 16) // 16 bytes to write { FM16B[7]=(unsigned char)(0xff&(dwFLASHADDR >> 16)); FM16B[8]=(unsigned char)(0xff&(dwFLASHADDR >> 8)); FM16B[9]=(unsigned char)(0xff&(dwFLASHADDR)); memcpy(&FM16B[10],bFLASH,16); CountWrite = sizeof(FM16B); memset(&FM16BOUT,0, sizeof(FM16BOUT)); FM16BOUT[0] = FM16B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM16B)-1; ii++) { if ((FM16B[ii] >='1' && FM16B[ii] <= '9') || (FM16B[ii] == '#')) { // needs to insert escape == # FM16BOUT[iii++] = '#'; } FM16BOUT[iii++] = FM16B[ii]; } FM16BOUT[iii++] = FM16B[ii]; fwrite(&FM16BOUT,iii,1, FOutf); iCounBytes = 0; } if (iCounBytes == 0) { memset(bFLASH,0,sizeof(bFLASH)); dwFLASHADDR = dwAddress; } bFLASH[iCounBytes++] = dwMySin; bFLASH[iCounBytes++] = DiffSinCos; dwAddress+=2; // two bytes } dwExtractOld = dwExtract; } fprintf(FOutC,"\nEND_OF_TABLE:"); fprintf(FOutC,"\n FlashAddr.dwVal += ((Square.dwMiddle)<<1);"); fprintf(FOutC,"\n PROC_2_BYTES();"); if (iCounBytes) { FM16B[7]=(unsigned char)(0xff&(dwFLASHADDR >> 16)); FM16B[8]=(unsigned char)(0xff&(dwFLASHADDR >> 8)); FM16B[9]=(unsigned char)(0xff&(dwFLASHADDR)); memcpy(&FM16B[10],bFLASH,16); CountWrite = sizeof(FM16B); memset(&FM16BOUT,0, sizeof(FM16BOUT)); FM16BOUT[0] = FM16B[0]; int ii = 1; int iii = 1; for (ii = 1; ii < sizeof(FM16B)-1; ii++) { if ((FM16B[ii] >='1' && FM16B[ii] <= '9') || (FM16B[ii] == '#')) { // needs to insert escape == # FM16BOUT[iii++] = '#'; } FM16BOUT[iii++] = FM16B[ii]; } FM16BOUT[iii++] = FM16B[ii]; fwrite(&FM16BOUT,iii,1, FOutf); iCounBytes = 0; } fclose(FOutf); fclose(FOutC); fclose(FOut1); fclose(FOutH); fclose(FOutF); #endif } else if(MyByte == '7') { ////////////////////////////////////////////////////////FLASH memory // program flash chip with calibration data FILE *DatCalX = fopen("MyDeg.HEX","rb"); int iCountWr = 0; if (DatCalX != NULL) { int iread = 0; printf("\n writing."); while(iread = fread(&bBuffer, 1, sizeof(bBuffer), DatCalX)) { WriteFileCom( hComHandle, bBuffer, iread, &lenwrt, NULL ); Sleep(50); iCountWr++; if ((iCountWr%16) == 0) printf("."); } fclose(DatCalX); // now confirm reading } } else if(MyByte == '8') { #define FLASH_16_READ "1=5 F\x05\x03\x00\x00\x00@\x10\x31" //#define FLASH_8_READ "1= F\x05\x03\x00\x00\x00@\x0c\x31" unsigned long uAddr = 0; FILE *DatCalXO = fopen("FLASH.OUT","wb"); int iCountWr = 0; unsigned long ui = 0; if (DatCalXO != NULL) { for (long ij = 0x5*256*256; ij < 0x10*256*256; ij+=16) { ui = ij; memcpy(bBuffer,FLASH_16_READ, sizeof(FLASH_16_READ)-1); bBuffer[8]=(unsigned char)(0xff&(ui >> 16)); bBuffer[9]=(unsigned char)(0xff&(ui >> 8)); bBuffer[10]=(unsigned char)(0xff&(ui)); bBuffer[100] = bBuffer[0]; bBuffer[101] = bBuffer[1]; bBuffer[102] = bBuffer[2]; bBuffer[103] = bBuffer[3]; bBuffer[104] = bBuffer[4]; int ii = 5; int iii = 5; for (ii = 5; ii < sizeof(FLASH_16_READ)-1-1; ii++) { if ((bBuffer[ii] >='1' && bBuffer[ii] <= '9') || (bBuffer[ii] == '#')) { // needs to insert escape == # bBuffer[100+iii] = '#'; iii++; } bBuffer[100+iii] = bBuffer[ii]; iii++; } bBuffer[100+iii] = bBuffer[ii]; iii++; WriteFileCom( hComHandle, &bBuffer[100], iii, &lenwrt, NULL ); Sleep(25); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); fwrite(&bBuffer[1], 16, 1, DatCalXO); uAddr++; if ((uAddr%256) ==0) printf("."); } fclose(DatCalXO); } } else if(MyByte == '9') { } else if(MyByte == 'Q') { //#define INIT_GYRO_Q "1=5` Q1" #define INIT_GYRO_Q "1=5' Q1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_Q, sizeof(INIT_GYRO_Q)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_Q)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); if (lenwrt == 0x17) // this is pic18F2321 printf("pic18f2321"); else printf("pic24hj64gp502"); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == '.') { long int CoefI; unsigned long int Z0; unsigned long int Y0; unsigned long int X0; unsigned long long lZ0; unsigned long long lY0; unsigned long long lX0; long int *sZ0; long int *sY0; long int *sX0; long long *lsZ0; long long *lsY0; long long *lsX0; float fX0; float fY0; float fZ0; float fCoefI; unsigned long ValQ; unsigned long ValX; unsigned long ValY; unsigned long ValZ; unsigned int ValSign; unsigned int ValMSB; float fValQ; float fValX; float fValY; float fValZ; float fModul; float fAngle; #define _Q_PROCESS 1 //#define INIT_GYRO_Q "1=5` Q1" #define INIT_GYRO_DOT "1=5'CQ1" for (int iC = 0; iC <16; iC++) //this will be =256 sec = 4.2(6) min = 1.0(6) degree earth rotation { memcpy(bBuffer,INIT_GYRO_DOT, sizeof(INIT_GYRO_DOT)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_DOT)-1, &lenwrt, NULL ); Sleep(4000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); int ii = 2; int iii = 2; double dDivider = 8000.0/(COEF_FOR_DEVISION+1); double Sensetivity = 8.0; double Sence = 14.375*8; if (iDoubleGyro) dDivider *= 2; if (lenwrt == 0x22) // this is 502 { CoefI = bBuffer[3]; CoefI <<=8; CoefI += bBuffer[4]; lX0 = bBuffer[5]; lX0 <<=8;lX0 += bBuffer[6]; lX0 <<=8;lX0 += bBuffer[7]; lX0 <<=8;lX0 += bBuffer[8];lX0 <<=8; lX0 += bBuffer[9]; lX0 <<=8;lX0 += bBuffer[10];lX0 <<=8;lX0 += bBuffer[11];lX0 <<=8;lX0 += bBuffer[12]; lY0 = bBuffer[13]; lY0 <<=8;lY0 += bBuffer[14];lY0 <<=8;lY0 += bBuffer[15];lY0 <<=8;lY0 += bBuffer[16];lY0 <<=8; lY0 += bBuffer[17]; lY0 <<=8;lY0 += bBuffer[18];lY0 <<=8;lY0 += bBuffer[19];lY0 <<=8;lY0 += bBuffer[20]; lZ0 = bBuffer[21]; lZ0 <<=8;lZ0 += bBuffer[22];lZ0 <<=8;lZ0 += bBuffer[23];lZ0 <<=8;lZ0 += bBuffer[24];lZ0 <<=8; lZ0 += bBuffer[25]; lZ0 <<=8;lZ0 += bBuffer[26];lZ0 <<=8;lZ0 += bBuffer[27];lZ0 <<=8;lZ0 += bBuffer[28]; lsX0 = (long long*)&lX0; lsY0 = (long long*)&lY0; lsZ0 =(long long *)&lZ0; fX0 = *lsX0;fY0 = *lsY0;fZ0 = *lsZ0;// fCoefI = CoefI;fX0/=fCoefI;fY0/=fCoefI;fZ0/=fCoefI; fX0/=256;fY0/=256;fZ0/=256; ii = bBuffer[32]; iii = (bBuffer[31]<<8)+bBuffer[30]; } else // this is 2321 { #ifdef _Q_PROCESS ValMSB = bBuffer[2]; ValMSB <<=8; ValMSB += bBuffer[3]; ValSign= bBuffer[4]; ValSign<<=8; ValSign+= bBuffer[5]; ValZ = bBuffer[ 6]; ValZ <<=8; ValZ+= bBuffer[ 7]; ValZ <<=8; ValZ += bBuffer[ 8]; ValZ <<=8;ValZ += bBuffer[ 9];fValZ=ValZ; ValY = bBuffer[10]; ValY <<=8; ValY+= bBuffer[11]; ValY <<=8; ValY += bBuffer[12]; ValY <<=8;ValY += bBuffer[13];fValY=ValY; ValX = bBuffer[14]; ValX <<=8; ValX+= bBuffer[15]; ValX <<=8; ValX += bBuffer[16]; ValX <<=8;ValX += bBuffer[17];fValX=ValX; ValQ = bBuffer[18]; ValQ <<=8; ValQ+= bBuffer[19]; ValQ <<=8; ValQ += bBuffer[20]; ValQ <<=8;ValQ += bBuffer[21];fValQ=ValQ; if (ValSign&0x0001) fValQ=-fValQ; if (ValSign&0x0002) fValX=-fValX; if (ValSign&0x0004) fValY=-fValY; if (ValSign&0x0008) fValZ=-fValZ; #else CoefI = bBuffer[3]; CoefI <<=8; CoefI += bBuffer[2]; X0 = bBuffer[4]; X0 <<=8;X0 += bBuffer[5]; X0 <<=8;X0 += bBuffer[6]; X0 <<=8;X0 += bBuffer[7]; Y0 = bBuffer[9]; Y0 <<=8;Y0 += bBuffer[10]; Y0 <<=8;Y0 += bBuffer[11]; Y0 <<=8;Y0 += bBuffer[12]; Z0 = bBuffer[14]; Z0 <<=8;Z0 += bBuffer[15]; Z0 <<=8;Z0 += bBuffer[16]; Z0 <<=8;Z0 += bBuffer[17]; sX0 = (long*)&X0; sY0 = (long*)&Y0; sZ0 =(long*)&Z0; fX0 = *sX0;fY0 = *sY0;fZ0 = *sZ0;// fCoefI = CoefI;fX0/=fCoefI;fY0/=fCoefI;fZ0/=fCoefI; #endif } //printf("\n X=%ld (%ld), Y=%ld(%ld), Z=%ld(%ld)", (*sX0)/CoefI,(*sX0)/*CoefI-XX0*/,(*sY0)/CoefI,(*sY0)/*CoefI-YY0*/,(*sZ0)/CoefI,(*sZ0)/*CoefI-ZZ0*/); //XX0 =(*sX0)/CoefI; YY0 =(*sY0)/CoefI; ZZ0 =(*sZ0)/CoefI; //if (XX0 > 0x7fff) // XX0 = 0x10000- XX0; //if (YY0 > 0x7fff) // YY0 = 0x10000- YY0; //if (ZZ0 > 0x7fff) // ZZ0 = 0x10000- ZZ0; //printf(" (%ld,%ld,%ld",(*sX0)/15,(*sY0)/15,(*sZ0)/15); #ifdef _Q_PROCESS printf("\nQ=%08x %08x %08x %08x ",ValQ,ValX,ValY,ValZ); if (ValMSB&01) fValQ += 256*256*256*256; if (ValMSB&02) fValX += 256*256*256*256; if (ValMSB&04) fValY += 256*256*256*256; if (ValMSB&06) fValZ += 256*256*256*256; fModul = sqrt(fValX*fValX + fValY*fValY + fValZ*fValZ +fValQ*fValQ); fModul = (256.0*256.0*256.0*256.0)/fModul; fValX *=fModul;fValY *=fModul;fValZ *=fModul;fValQ *=fModul; printf("m=%f ",fModul); fModul = sqrt(fValX*fValX + fValY*fValY + fValZ*fValZ +fValQ*fValQ); fModul = sqrt(fValX*fValX + fValY*fValY + fValZ*fValZ); if (fValQ >= (float)((long long)0x100000000)) fValQ = (float)((long long)0x100000000); fAngle = acos(fValQ/(256.0*256.0*256.0*256.0)); fAngle = (fAngle*2.0)*180.0/M_PI; //printf(" A=%f X=%f Y=%f Z=%f",fAngle, acos(fValX/fModul)/M_PI*180.0,acos(fValY/fModul)/M_PI*180.0,acos(fValZ/fModul)/M_PI*180.0); printf("A=%f %5.2f %5.2f %5.2f",fAngle, fValX/fModul,fValY/fModul,fValZ/fModul); #else fCoefI = CoefI; printf("\nX=%f Y=%f Z=%f t=%f (%d) U=%x T=%x", fX0/14.375/dDivider/Sensetivity,fY0/14.375/dDivider/Sensetivity,fZ0/14.375/dDivider/Sensetivity,fCoefI/dDivider,CoefI,ii,iii); #endif //printf("\nX=%f Y=%f Z=%f (%d)", fX0/14.375/fCoefI,fY0/14.375/fCoefI,fZ0/14.375/fCoefI,CoefI); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if((MyByte == 'I') || (MyByte == 'i')) // { //#define INIT_GYRO_I "1I\x07\x1e\x47\x0e\x2c 1 22" // 07 1e //#define INIT_GYRO_I "1I\x07\x1e\xb5\x00\x00\x00 1 22" //#define INIT_GYRO_I "1I\x01\x1e\xb5\x4b\x00\x11\x82\x28\x00 1 22" //#define INIT_GYRO_I "1I\x03\x1e\xb5\x4a\x00\x10\x00\x25\x00 1 22" //#define INIT_GYRO_I "1I\x01\x1a\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /2 == 4000 samples per sec (real reading was 343.5 //#define INIT_GYRO_I "1I\x01\x18\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /27 == 296 samples per sec (real reading was .... //#define INIT_GYRO_I "1I\x1a\x18\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /27 == 296 samples per sec value 01 instead 11 01 is *4 samples //#define INIT_GYRO_I "1I\x1a\x08\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /27 == 296 samples per sec value 11 instead 11 //#define INIT_GYRO_I "1I\x1a\x08\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /27 == 296 samples per sec value 00 14.375*8 //#define INIT_GYRO_I "1I\x1a\x00\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /27 == 296 samples per sec value 1 (???) ; 00=14.375*8 //#define INIT_GYRO_I "1I\x1a\x20\xb5\x4b\x00\x11\x82\x28\x00 1 22" // 8000 /8 == 1000 samples per sec value 00 14.375*8 //#define INIT_GYRO_I "I\x07\x00\xb5\x00\xb5\x11\x82\x28\x00@1" #define INIT_GYRO_I "1I\x03\x00\xb5\xb5\x31" #define INIT_GYRO_I_TOGETHER "1I\x03\x00\xb5\x00\x31" // 8000 /16 == 500 samples per sec value 00 14.375*8 //#define INIT_GYRO_I "1I\x0f\x00\xb5\xb5\x4b\x00\x11\x82\x28\x00 1 22" // XX reg 0x15 Fsample = Finternal / (divider+1), where Finternal is either 1kHz or 8kHz // XX reg 0x16 // 000 // 11 FS_SEL 3 ±2000 deg/sec Full scale selection for gyro sensor data // xxx DLPF_CFG Digital low pass filter configuration and internal sampling rate configuration // 0 256Hz 8kHz // 1 188Hz 1kHz // 2 98Hz 1kHz // 3 42Hz 1kHz // 4 20Hz 1kHz // 5 10Hz 1kHz // 6 5Hz 1kHz // 7 Reserved Reserved // XX reg 0x17 // x ACTL Logic level for INT output pin – 1=active low, 0=active high // x OPEN Drive type for INT output pin – 1=open drain, 0=push-pull // x LATCH_INT_EN Latch mode – 1=latch until interrupt is cleared, 0=50us pulse // x INT_ANYRD_2CLEAR Latch clear method – 1=any register read, 0=status register read only // 0 // x ITG_RDY_EN Enable interrupt when device is ready (PLL ready after changing clock source) // 0 // x RAW_RDY_EN Enable interrupt when data is available // putchI2C(0b10110101);=0xb5 // // XX reg 0x17 for second Gyro //for (int iC = 0; iC <256; iC++) { if (MyByte == 'i') { memcpy(bBuffer,INIT_GYRO_I_TOGETHER, sizeof(INIT_GYRO_I_TOGETHER)-1); bBuffer[2] = COEF_FOR_DEVISION; //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_I_TOGETHER)-1, &lenwrt, NULL ); } else { memcpy(bBuffer,INIT_GYRO_I, sizeof(INIT_GYRO_I)-1); bBuffer[2] = COEF_FOR_DEVISION; //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_I)-1, &lenwrt, NULL ); } Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if((MyByte == 'S') || (MyByte == 's') || (MyByte == 'k')) { #define INIT_GYRO_S "1S1 started" #define INIT_GYRO_k "1k\x18\x00\x00\x00\x00\x00\x00\x31 started" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_S, sizeof(INIT_GYRO_S)-1); iDoubleGyro = 0; if (MyByte == 'S') bBuffer[1]= 'S'; else { if (MyByte == 's') bBuffer[1]= 's'; else { memcpy(bBuffer,INIT_GYRO_k, sizeof(INIT_GYRO_k)-1); iDoubleGyro = 1; } } //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_S)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'P') { #define INIT_GYRO_P "1P1 stoped" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_GYRO_P, sizeof(INIT_GYRO_P)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_GYRO_P)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } ////////////////////////////////////////////////////////FLASH memory else if(MyByte == 'V') // program flash chip with calibration data { FILE *DatCalX = fopen("GYROFLASH.HEX","rb"); int iCountWr = 0; if (DatCalX != NULL) { int iread = 0; printf("\n writing."); while(iread = fread(&bBuffer, 1, sizeof(bBuffer), DatCalX)) { WriteFileCom( hComHandle, bBuffer, iread, &lenwrt, NULL ); Sleep(50); iCountWr++; if ((iCountWr%16) == 0) printf("."); } fclose(DatCalX); // now confirm reading } } else if(MyByte == 'v') // read all flash back { #define FLASH_8_READ "1=5 F\x05\x03\x00\x00\x00@\x0c\x31" //#define FLASH_8_READ "1= F\x05\x03\x00\x00\x00@\x0c\x31" unsigned long uAddr = 0; FILE *DatCalX = fopen("GYROFLASH.OUT","wb"); FILE *DatCalXO = fopen("FLASH.OUT","wb"); int iCountWr = 0; unsigned long ui = 0; if (DatCalX != NULL) { for (long ij = 0; ij < 2*256*256; ij+=16) { ui = uAddr; ui*=16; memcpy(bBuffer,FLASH_8_READ, sizeof(FLASH_8_READ)-1); bBuffer[8]=(unsigned char)(0xff&(ui >> 16)); bBuffer[9]=(unsigned char)(0xff&(ui >> 8)); bBuffer[10]=(unsigned char)(0xff&(ui)); bBuffer[100] = bBuffer[0]; bBuffer[101] = bBuffer[1]; bBuffer[102] = bBuffer[2]; bBuffer[103] = bBuffer[3]; bBuffer[104] = bBuffer[4]; int ii = 5; int iii = 5; for (ii = 5; ii < sizeof(FLASH_8_READ)-1-1; ii++) { if ((bBuffer[ii] >='1' && bBuffer[ii] <= '9') || (bBuffer[ii] == '#')) { // needs to insert escape == # bBuffer[100+iii] = '#'; iii++; } bBuffer[100+iii] = bBuffer[ii]; iii++; } bBuffer[100+iii] = bBuffer[ii]; iii++; WriteFileCom( hComHandle, &bBuffer[100], iii, &lenwrt, NULL ); Sleep(100); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); int iqq = 1; for(int iq = 1; iq> 16)); FM8B[8]=(unsigned char)(0xff&(ui >> 8)); FM8B[9]=(unsigned char)(0xff&(ui)); FM8B[10]=bBuffer[1]; FM8B[11]=bBuffer[2]; FM8B[12]=bBuffer[3]; FM8B[13]=bBuffer[4]; FM8B[14]=bBuffer[5]; FM8B[15]=bBuffer[6]; FM8B[16]=bBuffer[7]; FM8B[17]=bBuffer[8]; FM8B[18]=bBuffer[9]; FM8BOUT[0] = FM8B[0]; ii = 1; iii = 1; for (ii = 1; ii < sizeof(FM8B)-1; ii++) { if ((FM8B[ii] >='1' && FM8B[ii] <= '9') || (FM8B[ii] == '#')) { // needs to insert escape == # FM8BOUT[iii++] = '#'; } FM8BOUT[iii++] = FM8B[ii]; } FM8BOUT[iii++] = FM8B[ii]; if (ij < 2*256*256) fwrite(&FM8BOUT,iii,1, DatCalX); uAddr++; if ((uAddr%256) ==0) printf("."); } fclose(DatCalX); fclose(DatCalXO); } } else if(MyByte == 'E') // flash chip erase { #define INIT_FLASH_ERASE "\x31\x46\x01\x06\x46\x01\x60\x31" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_FLASH_ERASE, sizeof(INIT_FLASH_ERASE)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_FLASH_ERASE)-1, &lenwrt, NULL ); Sleep(15000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'F') { #define INIT_FLASH_1 "1=5` F\x05\x03\x00\x00\x00@\x04\x31" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_FLASH_1, sizeof(INIT_FLASH_1)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_FLASH_1)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'f') { #define INIT_FLASH_2 "1F\x01\x06\x46\x04\x20\x00\x00\x00\x31" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_FLASH_2, sizeof(INIT_FLASH_2)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_FLASH_2)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'Z') { //#define INIT_FLASH_2 "1F\x01\x06F\x04\x20\x00\x00\x00\x31" #define INIT_FLASH_3 "1F\x01\x06\x46\x0c\x02\x00\x00\x00\x01\x02\x03\x04\x05\x06\x07\x08\x31" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,INIT_FLASH_3, sizeof(INIT_FLASH_3)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(INIT_FLASH_3)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'A') { // device Id == 53 #define ACS_READ "1=5AC<\x53#\x32>\x06@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,ACS_READ, sizeof(ACS_READ)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(ACS_READ)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'a') { // 2d == 0000 0000 // 0 == link // 0 == autoslip // 1 == measure // 0 == sleep // 00== wakeup set frequency reading in sleep mode 00 - 8Hz 11 -1 Hz // 2e == 0000 0000 // 1 == data read interrupt // 0 == single tap interrupt // 0 == double tap interupt // 0 == activity interrupt // 0 == inactivity interupt // 0 == freefall interrupt // 0 == watermark int // 0 == overrun int // 2F == 0000 0000 // 0 == (0)data ready on pin int1 (1) data ready on pin Int2 // 30 == status bits of intrrrupts // // 31 == 0000 0000 DATA fromat // 0 == self test // 0 == spi bit (0) spi 3 wire (1) 4 wire // 1 == interrupt active low // 0 == dummy // 1 == full resolution // 0 == justify bit (0) // 00 == g range (00) +-2g (01) +-4G (10) +-8g (11) +-16g // 2d 2e 2f 30 31 #define ACS_INIT "1=5HC<\x53\x2d\x08\x80\x00\x00\x28@<\x53\x29>\x11@1" #define ACS_READ1 "1=5HC<\x53\x29>\x08@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,ACS_INIT, sizeof(ACS_INIT)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(ACS_INIT)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == ',') // asc readings { //for (int iC = 0; iC <256; iC++) { unsigned short int XH; unsigned short int YH; unsigned short int ZH; signed short int *pXH = (signed short int *)&XH; signed short int *pYH = (signed short int *)&YH; signed short int *pZH = (signed short int *)&ZH; double dXh; double dYh; double dZh; double dXH; double dYH; double dZH; double dYZH; double dXZH; double dXYH; double daXH; double daYH; double daZH; double dModul; dXH = 0; dYH = 0; dZH = 0; for (int iH = 0; iH < 100; iH ++) { memcpy(bBuffer,ACS_READ, sizeof(ACS_READ)-1); WriteFileCom( hComHandle, bBuffer, sizeof(ACS_READ)-1, &lenwrt, NULL ); Sleep(25); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); XH = (((unsigned short int)bBuffer[3])<<8)|(((unsigned short int)bBuffer[2])); YH = (((unsigned short int)bBuffer[5])<<8)|(((unsigned short int)bBuffer[4])); ZH = (((unsigned short int)bBuffer[7])<<8)|(((unsigned short int)bBuffer[6])); dXh = *pXH;dYh = *pYH;dZh = *pZH; dXH += dXh;dYH += dYh;dZH += dZh; } dModul = sqrt(dXH*dXH + dYH*dYH + dZH*dZH); //dXH /= dModul;dYH /= dModul;dZH /= dModul; daZH = atan(dXH/dYH)*180/M_PI; if (dXH > 0 && dYH > 0) daZH = -180 + daZH; else if (dXH < 0 && dYH >0) daZH = 180 + daZH; daYH = atan(dZH/dXH)*180/M_PI; if (dZH > 0 && dXH > 0) daYH = -180 + daYH; else if (dZH < 0 && dXH >0) daYH = 180 + daYH; daXH = atan(dYH/dZH)*180/M_PI; if (dYH > 0 && dZH > 0) daXH = -180 + daXH; else if (dYH < 0 && dZH >0) daXH = 180 + daXH; //if ((XH == 0xf000) || (XH == 0xf000) || (XH == 0xf000)) //{ // printf("\n error in readings"); //} //else { //dXH = acos(dXH)/M_PI*180;dYH = acos(dYH)/M_PI*180;dZH = acos(dZH)/M_PI*180; //if (*pXH<0) // dXH = -dXH;// - 180.0; //if (*pYH<0) // dYH = -dYH;// - 180.0; //if (*pZH<0) // dZH = -dZH;// - 180.0; printf("\n X=%f Y=%f Z=%f",dXH,dYH,dZH); printf("\n%f X=%f(%04x) Y=%f(%04x) Z=%f(%04x)",dModul,daXH,XH,daYH,YH,daZH,ZH); } //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } //printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'H') { #define MEGNETO_READ "1=5HC<\x1e\x03>\x06@1" //"1=5HC<\x1e\x00>\x0F@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,MEGNETO_READ, sizeof(MEGNETO_READ)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(MEGNETO_READ)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); lenwrt= RemoveESC(bBuffer, lenwrt); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == 'h') { // 18 == 0 00 000 00 // 00 == samples averaged ==0 // 110 == 75 per sec // 00=noral // 20 = 001 0 0000 == +=1.3Ga 1090 lsb Ga //#define MEGNETO_INIT "1=5HC<\x1e\x00\x18\x20\x00@1" // 00 = 000 0 0000 == +=0.88Ga 1370 lsb Ga //#define MEGNETO_INIT "1=5HC<\x1e\x00\x18\x00\x00@1" #define MEGNETO_INIT "1=5HC<\x1e\x00\x18\x00\x00@1" //for (int iC = 0; iC <256; iC++) { memcpy(bBuffer,MEGNETO_INIT, sizeof(MEGNETO_INIT)-1); //if ((iC != '1' ) && (iC != '@' ) && (iC != '<' ) && (iC != '>' ) && (iC != '=' ) && (iC != '#' )) //bBuffer[9] = iC; WriteFileCom( hComHandle, bBuffer, sizeof(MEGNETO_INIT)-1, &lenwrt, NULL ); Sleep(1000); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } else if(MyByte == ';') { #define MEGNETO_VALUES "1=5HC<\x1e\x03>\x06@1" //for (int iC = 0; iC <256; iC++) { unsigned short int XH; unsigned short int YH; unsigned short int ZH; signed short int *pXH = (signed short int *)&XH; signed short int *pYH = (signed short int *)&YH; signed short int *pZH = (signed short int *)&ZH; double dXh; double dYh; double dZh; double dXH; double dYH; double dZH; double dYZH; double dXZH; double dXYH; double daXH; double daYH; double daZH; double dModul; dXH = 0; dYH = 0; dZH = 0; for (int iH = 0; iH < 75; iH ++) { memcpy(bBuffer,MEGNETO_VALUES, sizeof(MEGNETO_VALUES)-1); WriteFileCom( hComHandle, bBuffer, sizeof(MEGNETO_VALUES)-1, &lenwrt, NULL ); Sleep(50); memset(bBuffer, 0, sizeof(bBuffer)); ReadFileCom( hComHandle, &bBuffer, sizeof(bBuffer), &lenwrt, NULL ); XH = (((unsigned short int)bBuffer[2])<<8)|(((unsigned short int)bBuffer[3])); ZH = (((unsigned short int)bBuffer[4])<<8)|(((unsigned short int)bBuffer[5])); YH = (((unsigned short int)bBuffer[6])<<8)|(((unsigned short int)bBuffer[7])); dXh = *pXH;dYh = *pYH;dZh = *pZH; dXH += dXh;dYH += dYh;dZH += dZh; } dModul = sqrt(dXH*dXH + dYH*dYH + dZH*dZH); //dXH /= dModul;dYH /= dModul;dZH /= dModul; daZH = atan(dXH/dYH)*180/M_PI; if (dXH > 0 && dYH > 0) daZH = -180 + daZH; else if (dXH < 0 && dYH >0) daZH = 180 + daZH; daYH = atan(dZH/dXH)*180/M_PI; if (dZH > 0 && dXH > 0) daYH = -180 + daYH; else if (dZH < 0 && dXH >0) daYH = 180 + daYH; daXH = atan(dYH/dZH)*180/M_PI; if (dYH > 0 && dZH > 0) daXH = -180 + daXH; else if (dYH < 0 && dZH >0) daXH = 180 + daXH; if ((XH == 0xf000) || (XH == 0xf000) || (XH == 0xf000)) { printf("\n error in readings"); } else { //dXH = acos(dXH)/M_PI*180;dYH = acos(dYH)/M_PI*180;dZH = acos(dZH)/M_PI*180; //if (*pXH<0) // dXH = -dXH;// - 180.0; //if (*pYH<0) // dYH = -dYH;// - 180.0; //if (*pZH<0) // dZH = -dZH;// - 180.0; printf("\n X=%f Y=%f Z=%f",dXH,dYH,dZH); printf("\n%f X=%f(%04x) Y=%f(%04x) Z=%f(%04x)",dModul,daXH,XH,daYH,YH,daZH,ZH); } //if (bBuffer[5] != 0xff) // printf("found = %d", iC); } //printf("\n bytes read =%d ===>\n%s\n<===",lenwrt,bBuffer); } } } CloseHandle(hComHandle); hComHandle = NULL; } } } else { // TODO: change error code to suit your needs _tprintf(_T("Fatal Error: GetModuleHandle failed\n")); nRetCode = 1; } fclose(DatOut); return nRetCode; }