/*********************************************************************** 2011 (C) Alex Dobrianski GYRO controller module This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see Design and development by Team "Plan B" is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License. http://creativecommons.org/licenses/by-sa/3.0/ ************************************************************************/ /*********************************************************************** see www.adobri.com for communication protocol spec ************************************************************************/ // [1 GYRO]->[2 MEM]-> [3 POW] -> [4 STM] -> [5 BT] -| // A | // -------------------------------------------------- #define MY_UNIT '5' #define MAX_COM_TOPOLOGY_UNIT '5' //#define ALLOW_RELAY_TO_NEW // needs to spec processor - C and ASM code is different #ifdef __18CXX #ifdef __16F88 #define _16F88 1 #endif #ifdef __16F884 #define _16F884 1 #endif #ifdef __18F2321 #define _18F2321 1 #endif #endif //////////////////////////////////////////// // listing in C30 // -Wa,-ahlsnd="$(BINDIR_)$(INFILEBASE).lst" // -g - debugging information // -O1 - optimization looks good // -02 and -O3 does something that code is not recognizable - can be dangerouse // -fpack-struct pack struct // -save-temps // -funroll-loops this will make loops big, ugly but fast //////////////////////////////////////////// // -g -Wall -save-temps -O1 -Wa,-ahlsnd="$(BINDIR_)$(INFILEBASE).lst" //////////////////////////////////////////// // it can be only master support: pic works in master mode only=> uncomment this line if // no multimaster support on a bus #define I2C_ONLY_MASTER 1 // master support done via interrupts and firmware - commenting next line and I2C will be a software work #define I2C_INT_SUPPORT 1 // define speed send data over com - sequence should be: // ptrSpeed = &AllGyro.TempL; pointer to the last transfwered byte -1 // LenSpeed = 17; bytes to transfer // SpeedSendLocked = 0; prepear lock mode for speed send // SpeedSendUnit = 0; unit at the end was not send et // SpeedSendWithESC = 1; does (or does not if == 0) send data using esc on units addreses // SpeedESCwas = 0; no esc on first byte et // SpeedSend = 1; LAST TO INITIATE // TXIE = 1; and run over interrupts #define SPEED_SEND_DATA 1 // if such fucntionality does not requare then commneting this line saved code in ISR // process as a qwords multi[lication #define _Q_PROCESS 1 // different processors: #ifdef _16F88 #undef SPEED_SEND_DATA #endif #ifdef _16F884 #endif #ifdef _18F2321 #endif // for additional (separated from SyncUART) support // if such functionality does not present then comment out this lines #define EXT_INT 1 #ifdef __PIC24H__ // SSCLOCK RA0(pin2), SSDATA_IN RA1(pin3), SSDATA_OUT RA2(pin9), SSCS RA3(pin10) //#define SSPORT LATAbits //#define SSCLOCK LATA0 //#define SSDATA_IN LATA1 //#define SSDATA_OUT LATA2 //#define SSCS LATA3 #define SSPORT PORTAbits #define SSCLOCK RA0 #define SSDATA_IN RA1 #define SSDATA_OUT RA2 #define SSCS RA3 #else // SSCLOCK RA7(pin9), SSDATA_IN RA6(pin10), SSDATA_OUT RA4(pin6), SSCS RA3(pin5) #define SSPORT PORTA #define SSCLOCK 7 #define SSDATA_IN 6 #define SSDATA_OUT 4 #define SSCS 3 #endif #include "commc0.h" #define INCLUDE_CALIBR 1 struct _Data_B0{ unsigned TimerPresc:1; unsigned Timer:1; unsigned SetBitsCmd:1; unsigned MinusPlusSet:1; unsigned Timer0Waiting:1; unsigned Reg15:1; unsigned Reg16:1; unsigned Reg17:1; unsigned Reg17Second:1; } DataB0; unsigned char ByteTimer; unsigned char REG15; unsigned char REG16; unsigned char REG17; unsigned char REG17Second; #pragma rambank RAM_BANK_1 ///////////////////////////////////////BANK 1////////////////////////// struct _Data_B1 { unsigned ExecInit:1; unsigned GyroRun:1; unsigned GyroGetStatus:1; unsigned GyroGetReply:1; unsigned GyroGetData:1; unsigned GyroStatusIs:1; unsigned GetGyroWait:1; unsigned DoIntegrCnt:1; unsigned ErrorInTemp:1; unsigned Run2Gyro:1; unsigned FirstGyroProcess:1; unsigned RstGyroWithVal:1; #ifdef INCLUDE_CALIBR unsigned Calibration:1; unsigned SecondCalibr:1; unsigned FirstSkip:1; #endif } DataB1; struct GYRO_DATA { unsigned char CalibrUnit; unsigned char TempH; unsigned char TempL; #ifdef __PIC24H__ union { struct { #endif unsigned char ZZ0; unsigned char ZZL; unsigned char ZZH; unsigned char ZZHH; unsigned char ZZHHH; #ifdef __PIC24H__ }; struct { long long ZZ; }; }; #endif #ifdef __PIC24H__ union { struct { #endif unsigned char YY0; unsigned char YYL; unsigned char YYH; unsigned char YYHH; unsigned char YYHHH; #ifdef __PIC24H__ }; struct { long long YY; }; }; #endif #ifdef __PIC24H__ union { struct { #endif unsigned char XX0; unsigned char XXL; unsigned char XXH; unsigned char XXHH; unsigned char XXHHH; #ifdef __PIC24H__ }; struct { long long XX; }; }; #endif #ifdef __PIC24H__ unsigned int IntegrCount; #else #ifndef _16F88 unsigned char IntegrCount2; unsigned char IntegrCount; #endif #endif #ifdef __PIC24H__ union { struct { unsigned char GyroByte0; unsigned char GyroByteL; int HByte; #else unsigned char HByte; #endif #ifdef __PIC24H__ }; struct { long GyroByte; }; }; #endif char GyroDataIs; } AllGyro; #ifdef __PIC24H__ #define HBYTE AllGyro.GyroByte #else #define HBYTE AllGyro.HByte #endif #ifndef _16F88 struct MyQWord { union { long long qVal; unsigned long long uqVal; struct { unsigned int wValL00; unsigned long dwMiddle; unsigned int wValH00; }; struct { unsigned long dwValL; unsigned long dwValH; }; struct { unsigned long dw2ValL; long lValH; }; struct { unsigned int wVal0; unsigned int wVal1; unsigned int wVal2; unsigned int wVal3; }; struct { unsigned int w2Val0; unsigned int w2Val1; unsigned int w2Val2; int iVal3; }; struct { unsigned char bVal0; unsigned char bVal1; unsigned char bVal2; unsigned char bVal3; unsigned char bVal4; unsigned char bVal5; unsigned char bVal6; unsigned char bVal7; }; struct { unsigned char b2Val0; unsigned char b2Val1; unsigned char b2Val2; unsigned char b2Val3; unsigned char b2Val4; unsigned char b2Val5; unsigned char b2Val6; char sVal7; }; }; }; struct Readings { union { long lVal; unsigned long dwVal; struct { unsigned char bValL; unsigned char bValH; int iValHH; }; struct { unsigned int iValL; int iValHH0; }; struct { unsigned char bValL; unsigned char bValH; unsigned char bValHH; unsigned char bValHHH; }; }; }; //unsigned char dddddebug; //unsigned char ddddebug; //unsigned char dddebug; //unsigned char ddebug; #define MAX_AVER 16 struct Vectors { long V1X; long V1Y; long V1Z; int iV1Counts; int v1x; int v1y; int v1z; int iShiftV1; unsigned long V1Sq; //should be no more then 256*256 readings if vectors are avaraged long V2X; long V2Y; long V2Z; int iV2Counts; int v2x; int v2y; int v2z; int iShiftV2; unsigned long V2Sq; long V1xV2; // scalar multiplication long V1pV2X; // vector multiplication long V1pV2Y; long V1pV2Z; unsigned long long V1pV2Sq; unsigned long CosAdiv2; unsigned long SinAdiv2; union { unsigned int SignV1pV2; struct { unsigned SignV1xV2:1; unsigned SignV1pV2X:1; unsigned SignV1pV2Y:1; unsigned SignV1pV2Z:1; }; }; unsigned int DataIs; signed int XR[MAX_AVER]; signed int YR[MAX_AVER]; signed int ZR[MAX_AVER]; unsigned int iCount; long XXAve; long YYAve; long ZZAve; union { signed int iRead; struct { unsigned char ReadL; signed char ReadH; } }; union { unsigned int Type; struct { unsigned TypeAcsOrMagn:1; // 0 == Acselrometr 1 = Magnetometr unsigned TypeSetV1:1; // 0 == inital V1 was not set yet 1 = initial avarage was set unsigned TypeCalcQ:1; // needs to calculte Quaternion rotation btw V1 and V2 unsigned TypeCorectGyro:1; // needs to correct Gyro readings by Acselerometer or Magnetometr unsigned TypeCorectEarth:1;// needs to correct Gyro by earth rotation (needs both Acselerometer and magnetometr running) unsigned TypeDoIntergal:1; // needs to intergrate acselerometer to get achived position unsigned TypeChekIntergalSign:1; // needs to detect changes of sign of a derivetive for magnetometr and store time // this time will be detection of apogee, perigee, points of crossing equator } }; unsigned long long XIntergral; unsigned long long YIntergral; unsigned long long ZIntergral; //unsigned long long TimeChngSign }; struct Quaternion // on pic multiplication DWORD=WORD*WORD it is just one command == multiplication faster then addition 2-3 times { unsigned long q; // represent cos(Alfa/2) values: <=0 (0==90 degree) <0xffffffff == 1-2.3283064370807973754314699618685e-10; if value == 1.0 then q=0 & msbX=1; unsigned long x; // represent rotation vector, sign stored in Sx; unsigned long y; // represent rotation vector, sign stored in Sy; unsigned long z; // represent rotation vector, sign stored in Sz; union { unsigned int Sign; struct { unsigned Sq:1; // sign of q component unsigned Sx:1; // sign of x unsigned Sy:1; // sign of y unsigned Sz:1; // sign of z }; }; union { unsigned int MSB; struct { unsigned msbQ:4; unsigned msbX:4; unsigned msbY:4; unsigned msbZ:4; }; }; }; // #define offQ1 0 #define offX1 4 #define offY1 8 #define offZ1 12 #define offQ2 0 #define offX2 4 #define offY2 8 #define offZ2 12 #define oQ1 0 #define oX1 4 #define oY1 8 #define oZ1 12 #define oQ2 0 #define oX2 4 #define oY2 8 #define oZ2 12 // // Qn = + Q1*Q2 - X1*X2 - Y1*Y2 - Z1*Z2 // Xn = + X1*Q2 + Q1*X2 + Y1*Z2 - Z1*Y2 // Yn = + Y1*Q2 + Q1*Y2 + Z1*X2 - X1*Z2 // Zn = + Z1*Q2 + Q1*Z2 + X1*Y2 - Y1*X2 // or by definition: // Qn = Q1*Q2 - X1*X2 - Y1*Y2 - Z1*Z2 // Xn = Q1*X2 + Q2*X1 + Y1*Z2 - Z1*Y2 // Yn = Q1*Y2 + Q2*Y1 + Z1*X2 - X1*Z2 // Zn = Q1*Z2 + Q2*Z1 + X1*Y2 - Y1*X2 static unsigned int SignMult = 0b0010100001001110; // multiplication sign helper // sequence: // 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 => 0100 // bacward oreder 0010100001001110 static int OffsetQ[32] = { offQ1-oQ2, offQ2-oQ2, offQ1-oX1, offQ2-oX2, offX1-oY1, offX2-oY2, offY1-oZ1, offY2-oZ2, offZ1-oX1, offZ2-oQ2, offX1-oQ1, offQ2-oX2, offQ1-oZ1, offX2-oY2, offZ1-oY1, offY2-oZ2, offY1-oY1, offZ2-oQ2, offY1-oZ1, offQ2-oX2, offZ1-oQ1, offX2-oY2, offQ1-oX1, offY2-oZ2, offX1-oZ1, offZ2-oQ2, offZ1-oY1, offQ2-oX2, offY1-oX1, offX2-oY2, offX1-oQ1, offY2-oZ2 }; // then backwards order //static int OffsetQ[32] = { //offZ2-oY2,offQ1-oX1, offY2-oX2,offX1-oY1, offX2-oQ2,offY1-oZ1, offQ2-oZ2,offZ1-oX1, //offZ2-oY2,offX1-oQ1, offY2-oX2,offQ1-oZ1, offX2-oQ2,offZ1-oY1, offQ2-oY2,offY1-oY1, //offZ2-oY2,offY1-oZ1, offY2-oX2,offZ1-oQ1, offX2-oQ2,offQ1-oX1, offQ2-oZ2,offX1-oZ1, //offZ2-oY2,offZ1-oY1, offY2-oX2,offY1-oX1, offX2-oQ2,offX1-oQ1, offQ2, offQ1 // }; struct SUB_G { #ifdef __PIC24H__ union { struct { #endif unsigned char xxL; // 0 unsigned char xxH; // 1 unsigned char xxHH; // 2 unsigned char xxHHH; // 2 unsigned char yyL; unsigned char yyH; unsigned char yyHH; unsigned char yyHHH; unsigned char zzL; unsigned char zzH; unsigned char zzHH; unsigned char zzHHH; WORD iCount; #ifdef __PIC24H__ }; struct { long xx; long yy; long zz; unsigned int iCount; }; }; #endif }; //struct SUB_G DoubleG; // x = x1-y2; y = y1-x2; z = z1-z2; this will be double value of Gyros reading //struct SUB_G DiffG; // x = x1+y2; y = y1+x2; z = z1+z2; this will be a difference btw two gyro struct SUB_G ZeroAvDblG; // stored Sum from two average value when Gyro has 0 == calibration for gyro = // xxL,xxH, xxHH has to be constantly minused from two gyro subtruct values struct SUB_G ZeroAvDiffG; // stored difference average when Gyro was 0 struct SUB_G DiffAvGOld; // when DiffAvG reached 256 that value has to be minused from ZeroDoubleG // and DiffAvGOld = DiffAvG => gyro will be adjusted each 1/4 of a second // TBD adjustment can be smoozed by adding more storage //struct SUB_G DblAvG; // 256 avarage of x = x1-y2; y = y1-x2; z = z1-z2; //struct SUB_G DiffAvG; // 256 avarage of x = x1+y2; y = y1+x2; z = z1+z2; #ifndef _Q_PROCESS struct SUB_G StoreSub[60]; struct SUB_G StoreAdd[60]; long long OldAllGyroXX; long long OldAllGyroYY; long long OldAllGyroZZ; #endif int iptrStore; unsigned char iRstPtr; unsigned char RstType; unsigned char RstXXL; unsigned char RstXXH; unsigned char RstYYL; unsigned char RstYYH; unsigned char RstZZL; unsigned char RstZZH; struct Readings OffsetSin; struct Readings OffsetCos; struct Readings FlashAddr; struct MyQWord Square; struct Readings ValCorrect; struct Readings ValCur; struct Readings ValX; struct Readings ValY; struct Readings ValZ; unsigned int NReadings; struct Quaternion QgyroRead; // rotation by Gyro in time slice (data read from Gyro) struct Quaternion Qgyro; // rotation from (0,0,g0) to curent direction by Gyro struct Quaternion QGFunc[16]; int CountQGFunc; VOLATILE unsigned long QgyroM; // modulus of Q struct Vectors Acs; struct Quaternion QAcs; // rotation from initial V1 of acselerometr till V2 struct Vectors Magn; struct Quaternion QMagn; // rotation from initial V1 of magnetometr till V2 struct Vectors *VAcsOrMagn; // processing vecor pointer -ether acselerometer or magnitometr struct Quaternion QCorrection; // correction for a gyro struct Quaternion QCorr0; // initial correction by Gyro struct Quaternion QCopy; // Quaternion copy for output #endif #pragma rambank RAM_BANK_3 /////////////////////////////////////////////BANK 3////////////////////// struct _DataB3{ unsigned FlashWrite:1; unsigned FlashWriteLen:1; unsigned FlashRead:1; } DataB3; unsigned char CountWrite; #ifdef INCLUDE_CALIBR void InitCalibr(); unsigned char ProcessCalibr(); unsigned char iSampleL; unsigned char iSampleH; unsigned char iSampleHH; unsigned char i8; #endif #pragma rambank RAM_BANK_1 ///////////////////////////////////////BANK 1////////////////////////// // // additional variables: #include "commc1.h" // // additional code: unsigned char CallBkComm(void); // return 1 == process queue; 0 == do not process; // 2 = do not process and finish process 3 == process and finish internal process // in case 0 fucntion needs to pop queue byte by itself unsigned char CallBkI2C(void); // return 1 == process queue; 0 == do not process; // 2 = do not process and finish process 3 == process and finish internal process // in case 0 fucntion needs to pop queue byte by itself unsigned char CallBkMain(void); // 0 = do continue; 1 = process queues void Reset_device(void); void ShowMessage(void); void ProcessCMD(unsigned char bByte); unsigned char getchI2C(void); void putch(unsigned char simbol); void putchWithESC(unsigned char simbol); unsigned char getch(void); void main() { unsigned char bWork; //unsigned long iMy1; //unsigned long iMy2; //unsigned long iMy3; //unsigned long dwMy4; //iMy1 = 0x1234; //iMy2 = 0xabc; //iMy3 = iMy1 + iMy2; //dwMy4 = iMy1 + iMy2; //dwMy4 = iMy1 * iMy2; if (POR_) // this is can be sync of a timer from MCLR { if (TimerB1.SetSyncTime) { #ifdef __PIC24H__ TMR2 = 0; #else TMR1L = 0; // must be delay in 2MHz clock TMR1H = 0; #endif TMR130 = setTMR130; TMR1SEC = setTMR1SEC; TMR1MIN = setTMR1MIN; TMR1HOUR = setTMR1HOUR; TMR1DAY = setTMR1DAY; } } Reset_device(); // needs to check what is it: //if (TO) // Power up or MCLR { // Unit == 1 Gyro UnitADR = '1'; //UnitADR = '8'; //UnitADR = '4'; #include "commc6.h" DataB0.TimerPresc = 0; DataB0.Timer = 0; DataB0.Timer0Waiting = 0; DataB0.SetBitsCmd = 0; DataB0.MinusPlusSet = 0; DataB0.Reg15 = 0; DataB0.Reg16 = 0; DataB0.Reg17 = 0; DataB0.Reg17Second = 0; DataB1.ExecInit = 0; DataB1.GyroRun = 0; AllGyro.XXHHH = 0; AllGyro.XXHH = 0; AllGyro.XXH = 0; AllGyro.XXL = 0; AllGyro.XX0 = 0; AllGyro.YYHHH = 0; AllGyro.YYHH = 0; AllGyro.YYH = 0; AllGyro.YYL = 0; AllGyro.YY0 = 0; AllGyro.ZZHHH = 0; AllGyro.ZZHH = 0; AllGyro.ZZH = 0; AllGyro.ZZL = 0; AllGyro.ZZ0 = 0; #ifdef __PIC24H__ AllGyro.GyroByte0 = 0; AllGyro.HByte = 0; #endif //DataB1.GyroGetStatus = 0; DataB1.GyroGetReply = 0; DataB1.GyroGetData = 0; DataB1.GyroStatusIs = 0; //DataB1.GyroGetData = 0; AllGyro.GyroDataIs = 0; DataB1.GetGyroWait = 0; DataB1.Run2Gyro = 0; #ifndef _16F88 AllGyro.IntegrCount = 0; #ifndef __PIC24H__ AllGyro.IntegrCount2 = 0; #endif //ddebug = 0; iRstPtr = 0; FSR_REGISTER = &ZeroAvDblG; bWork = 2*sizeof(ZeroAvDblG); // zero also ZeroAvDiffG CLEAN_1: PTR_FSR = 0; FSR_REGISTER++; if (--bWork) goto CLEAN_1; #endif DataB1.RstGyroWithVal = 0; DataB1.GyroRun = 0; #ifdef INCLUDE_CALIBR DataB1.Calibration = 0; DataB1.SecondCalibr = 0; #endif DataB3.FlashWrite = 0; DataB3.FlashWriteLen = 0; DataB3.FlashRead = 0; #ifdef SSPORT CS_HIGH; bclr(SSPORT,SSCLOCK); bclr(SSPORT,SSDATA_IN); #endif //SubG[0].xxL = 2; //SubG[0].xxH = 0; //FSR_REGISTER = (char*)&SubG[0].xxL; //W = SubG[0].xxH; //PTR_FSR -= W; // if no borrow then Carry is set // if borrow then Carry is clear } Main.ExtInterrupt = 0; // clean interrupts Main.ExtInterrupt1 = 0; Main.ExtInterrupt2 = 0; Main.ExtFirst = 1; #ifndef __PIC24H__ PEIE = 1; // bit 6 PEIE: Peripheral Interrupt Enable bit // 1 = Enables all unmasked peripheral interrupts // 0 = Disables all peripheral interrupts GIE = 1; // bit 7 GIE: Global Interrupt Enable bit // 1 = Enables all unmasked interrupts // 0 = Disables all interrupts RBIF = 0; #endif ShowMessage(); //bitset(PORTA,4); //bitset(PORTA,3); //bitset(SSPCON,4); // set clock high; #include "commc7.h" } // at the end will be Sleep which then continue to main #define SPBRG_9600 51 #define SPBRG_19200 25 #define SPBRG_19200_32MHZ 103 #define SPBRG_38400 12 #define SPBRG_57600 8 #define SPBRG_57600_32MHZ 33 #define SPBRG_57600_40MIPS 172 #define SPBRG_115200_40MIPS 85 // for pic18f2321 //#define SPBRG_SPEED SPBRG_57600_32MHZ // for pic24hj64gp502 #define SPBRG_SPEED SPBRG_57600_40MIPS #include "commc2.h" // additional code: // GYRO addr is 0x68 // reg 0 == 0x69 // reg 15 == 0x00 // reg 16 == 0x00 // reg 17 == 0x00 must be 0x18 // reg 1A == 0x01 // reg 1B == // 2a 2b 2c 2d 2e 2f 30 31 // acs reg 29 = 00 00 00 0a 00 00 00 02 00 void Reset_device(void); void ShowMessage(void); // ASCI // @ == 0 // A == 1 // B == 2 // C == 3 // D == 4 // E == 5 // F == 6 // G == 7 // H == 8 // I == 9 // : == 0xA J // ; == 0xB K // < == 0xc L // = == 0xd M // > == 0xe N // ? == 0xf O #define SAMPLES_PER_SEC 250 #ifdef __PIC24H__ #define CONVER_TO_INT(x) ((int)((signed char)x)) int findQBitLength(long long * ptrX) { int iShift = 64; int * ptrInt = ((int*)ptrX)+3; if ((*((unsigned int*)ptrInt))&0x8000) { while((iShift&0xf) ==0) { if (!((*((unsigned int*)ptrInt))&0x8000)) break; //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift <= 0) break; } } else { while((iShift&0xf) ==0) { if ((*((unsigned int*)ptrInt))&0x8000) break; //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift <= 0) break; } } return iShift; } int findBitLength(long * ptrX) { int iShift = 32; int * ptrInt = ((int*)ptrX)+1; if ((*((unsigned int*)ptrInt))&0x8000) { while((iShift&0xf) ==0) { if (!((*((unsigned int*)ptrInt))&0x8000)) break; //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift <= 0) break; } } else { while((iShift&0xf) ==0) { if ((*((unsigned int*)ptrInt))&0x8000) break; //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift <= 0) break; } } return iShift; } ///////////////////////////////////////////////////////////////////////////// // Temp = One / Two // One = One % Two // One is double size // rsa_dwlen - RSA len ///////////////////////////////////////////////////////////////////////////// // rsa_dwlen = 4 for 8 byts /* #define rsa_dwlen 4 void MOD_DWORD(unsigned int *Temp, unsigned int *One, unsigned int *Two) // rsa_dwlen = 4 { // make some spaces before and after 16+...+16 unsigned char bU[ 8 + 2*sizeof(unsigned int)]; unsigned char bV[2*sizeof(unsigned int ) + 8]; unsigned char bTemp[2*sizeof(unsigned int ) + 8]; const unsigned long B = 0x10000L; unsigned long D; unsigned long qTilda; unsigned long rTilda; unsigned long dwvnm1; unsigned long dwvnm2; unsigned int *dwU = (unsigned int*)&bU; unsigned int *dwV = (unsigned int*)&bV; int iDBits; int j; unsigned int *dwRes;// = (unsigned long*)&Temp[rsa_dwlen -1]; *(unsigned long*)&dwU[0] = 0; memcpy(&bU[sizeof(unsigned int)], One, rsa_dwlen * sizeof(unsigned int)*2); *(unsigned long*)&dwU[rsa_dwlen *2+2] = 0; // now U will be correct pointer dwU = (unsigned int*)&bU[sizeof(unsigned long)]; *(unsigned long*)&dwV[0] = 0; memcpy(&bV[sizeof(unsigned int)], Two, rsa_dwlen * sizeof(unsigned int)); *(unsigned long*)&dwV[rsa_dwlen + 2] = 0; // now V will be correct pointer dwV = (unsigned int*)&bV[sizeof(unsigned long)]; int n,m,i; // calc m for (i = rsa_dwlen*2 - 1, m = rsa_dwlen * 2; i >= 0; i--, m--) { if (dwU[i]) break; } if (m == 0) // this is attempt to One = 0 % Two result 0 return; // calc n for (i = rsa_dwlen - 1, n = rsa_dwlen; i >= 0; i--, n--) { if (dwV[i]) break; } if (n == 0) // this will be attempt One = One%0 (division on 0) not defined; left as it is return; // caclucale m for formula u[n+m-1]u[n+m-2]...u[0] and v[n-1]v[n-2]...v[0] m = m-n; if (m < 0) { memset(Temp, 0, rsa_dwlen * sizeof(unsigned int)*2); return; } // step D1: normalization. caulation D = b/(v[n-1]+1) D = B/(dwV[n-1]+1); // get good D = if it is 1 - nothing to do if (D==1) { iDBits = 0; } else { // get 8 bytes (from [n-1] needs one dword from left unsigned long DTillda = *(unsigned long*)&dwV[n-1 -1]; for (iDBits = 0; iDBits < sizeof(unsigned int)*8;iDBits++) { unsigned long DTillda1 = DTillda>>(sizeof(unsigned int)*8 - iDBits); if (DTillda1 >= (B/2)) break; } // siutable D (and iDBits) found now calc u[m+n]u[m+n-1]...u[0] and v[n-1][v[n-2]...v[0] // u will be shifted left to enpty space v will be shifted and will no go far then v[n-1] mSHLDW((unsigned int *)bU, iDBits, rsa_dwlen+2); mSHLDW((unsigned int *)bV, iDBits, rsa_dwlen); } dwvnm1 = ((unsigned long)(dwV[n-1])); dwvnm2 = ((unsigned long)(dwV[n-2])); // carfull!! div can produced high bit - for it storage needs to have 1 qword extra dwRes = (unsigned int*)&Temp[m]; if (rsa_dwlen- m + 2 > 0) memset(dwRes, 0, sizeof(unsigned int)*(rsa_dwlen- m + 2)); // step D1: assign j=m for (j = m; j>=0; j--) { // step D3: calculation qTilda // assign qTilda = (u[j+n]*b +u[j+n-1])/v[n-1] // assign rTilda = (u[j+n]*b +u[j+n-1])%v[n-1] //qTilda = (((((unsigned long long )(dwU[j+n]))<<32) + ((unsigned long long)(dwU[j+n-1])))) / ((unsigned long long)(dwV[n-1])); //rTilda = (((((unsigned long long)(dwU[j+n]))<<32) + ((unsigned long long)(dwU[j+n-1])))) % ((unsigned long long)(dwV[n-1])); if ((*(unsigned long*)(&dwU[j+n-1])) < dwvnm1) { // step D8: div = q[m]q[m-1]q[0] *(dwRes--) = 0; continue; } qTilda = (*(unsigned long*)(&dwU[j+n-1])) / dwvnm1; rTilda = (*(unsigned long*)(&dwU[j+n-1])) % dwvnm1; if (qTilda) { //while( (qTilda == B) || ( (qTilda*((unsigned long long)(dwV[n-2]))) > ((rTilda<<32)+((unsigned long long)(dwU[j+n-2]))) ) ) while( (qTilda == B) || ( (qTilda*dwvnm2) > ((rTilda<<32)+((unsigned long)(dwU[j+n-2]))) ) ) { qTilda--; //rTilda = rTilda + ((unsigned long long)(dwV[n-1])); rTilda = rTilda + dwvnm1; if (rTilda < B) continue; else break; } // step D4: multiplication and subtraction: // assign U[j+n]u[j+n-1]...u[j] = U[j+n]u[j+n-1]...u[j] - qTilda* v[n-1]v[n-2]...v[0] *(unsigned long*)(&bTemp) = (*dwV) * qTilda; if ((*(unsigned long*)&dwU[j] = *(unsigned long*)&dwU[j]- *(unsigned long*)(&bTemp)) < 0) { // step D5: check result == negative value? // step D6: adjust by add //adddw2(&dwU[j], &dwU[j], dwV, n/2+1); *(unsigned long*)&dwU[j] += (*dwV); } } // step D8: div = q[m]q[m-1]q[0] *(dwRes--) = qTilda; // step D7: next j = j -1; } // step D8: calculate remainder: // r= u[n-1]u[n-2]...u[0] / d if (iDBits) mSHRDW(dwU, iDBits, rsa_dwlen); memcpy(One, dwU,2*rsa_dwlen*sizeof(unsigned int)); }*/ //#endif // Res = w3:w2:w1:w0 = (B|A)*C unsigned long long mul3w_w(unsigned int A, unsigned long B,unsigned int C) { // A = w0 // B = w2:w3 // C = w1 <- do not ask why // Res = w3:w2:w1:w0 = (B|A)*C //if ((A == 0) && (B==0)) // return 0; __asm__ volatile ("push.d w4");// __asm__ volatile ("mul.uu w1,w2,w4"); // result will be w3:w2:w1:w0 now [w5:w4] = Bl*c __asm__ volatile ("mul.uu w1,w3,w2"); // [w3:w2] = Bh*c __asm__ volatile ("mul.uu w1,w0,w0"); // [w1:w0] = C * A __asm__ volatile ("add w4,w1,w1"); // __asm__ volatile ("addc w5,w2,w2"); // __asm__ volatile ("addc w3,#0,w3"); // __asm__ volatile ("pop.d w4");// ; restore regs } unsigned long long muldw_dw(unsigned long A, unsigned long B) { // A = w1:w0 // B = w3:w2 // Res = w3:w2:w1:w0 = A*B __asm__ volatile ("push.d w8");// ; save regs __asm__ volatile ("push.d w6");// __asm__ volatile ("push.d w4");// __asm__ volatile ("mul.uu w1,w3,w6"); // result will be w7:w6:w5:w4 now w6:w7 = A.hhhh * B.hhhh __asm__ volatile ("mul.uu w0,w2,w4"); // A.llll * B.llll => w5:w4 now w4:w5 = A.llll * B.llll __asm__ volatile ("mul.uu w1,w2,w8"); // A.hhhh * B.llll => w9:w8 now w8:w9 = A.hhhh * B.llll __asm__ volatile ("add w8,w5,w5"); // now w5=w5+w8 __asm__ volatile ("addc w9,w6,w6"); // now w6=w6+w9 +Carry __asm__ volatile ("addc w7,#0,w7"); // now w7+=Carry __asm__ volatile ("mul.uu w3,w0,w8"); // A.llll * B.hhhh => w9:w8 now w8:w9 = A.llll * B.hhhh __asm__ volatile ("add w8,w5,w5"); // now w5 = w8+w5 __asm__ volatile ("addc w9,w6,w2"); // now w2 = w9+w6+carry __asm__ volatile ("addc w7,#0,w3"); // now w3 = w7+carry __asm__ volatile ("mov.d w4,w0"); // now w0:w1 = w4:w5 __asm__ volatile ("pop.d w4");// ; restore regs __asm__ volatile ("pop.d w6");// __asm__ volatile ("pop.d w8");// } unsigned long mul3_3(unsigned long A, unsigned long B) { // A = w1:w0 // B = w3:w2 // Res = w1:w0 = A*B>>24 __asm__ volatile ("push.d w8");// ; save regs __asm__ volatile ("push.d w6");// __asm__ volatile ("push.d w4");// __asm__ volatile ("mul.uu w1,w3,w6"); // result will be w7:w6:w5:w4 now w6:w7 = A.hhhh * B.hhhh __asm__ volatile ("mul.uu w0,w2,w4"); // A.llll * B.llll => w5:w4 now w4:w5 = A.llll * B.llll __asm__ volatile ("mul.uu w1,w2,w8"); // A.hhhh * B.llll => w9:w8 now w8:w9 = A.hhhh * B.llll __asm__ volatile ("add w8,w5,w5"); // now w5=w5+w8 __asm__ volatile ("addc w9,w6,w6"); // now w6=w6+w9 +Carry __asm__ volatile ("addc w7,#0,w7"); // now w7+=Carry __asm__ volatile ("mul.uu w3,w0,w8"); // A.llll * B.hhhh => w9:w8 now w8:w9 = A.llll * B.hhhh __asm__ volatile ("add w8,w5,w0"); // now w0 = w8+w5 __asm__ volatile ("addc w9,w6,w1"); // now w1 = w9+w6+carry __asm__ volatile ("addc w7,#0,w7"); // now w7 = w7+carry __asm__ volatile ("sl w1,#8,w2"); // now w2 = (hhhh<<8) __asm__ volatile ("lsr w0,#8,w0"); // now w0 = llll>>8 __asm__ volatile ("ior w2,w0,w0"); // now w0 = (hhhh<<8) | llll>>8 __asm__ volatile ("lsr w1,#8,w1"); // now w1 = (hhhh>>8) __asm__ volatile ("sl w7,#8,w2"); // now w2 = HhHh<<8 __asm__ volatile ("ior w2,w1,w1"); // now w1 = (HhHh<<8) | hhhh>>8 __asm__ volatile ("pop.d w4");// ; restore regs __asm__ volatile ("pop.d w6");// __asm__ volatile ("pop.d w8");// } unsigned long my_umul(unsigned long A, unsigned long B) { // A = w1:w0 // B = w3:w2 // Res = A*B >>32 __asm__ volatile ("push.d w8");// ; save regs __asm__ volatile ("push.d w6");// __asm__ volatile ("push.d w4");// __asm__ volatile ("mul.uu w1,w3,w6"); // result will be w7:w6:w5:w4 now w7:w8 = A.hhhh * B.hhhh __asm__ volatile ("mul.uu w0,w2,w4"); // A.llll * B.llll => w5:w4 __asm__ volatile ("mul.uu w1,w2,w8"); // A.hhhh * B.llll => w9:w8 __asm__ volatile ("add w8,w5,w5"); // __asm__ volatile ("addc w9,w6,w6"); __asm__ volatile ("addc w7,#0,w7"); __asm__ volatile ("mul.uu w3,w0,w8"); // A.llll * B.hhhh => w9:w8 __asm__ volatile ("add w8,w5,w5"); // __asm__ volatile ("addc w9,w6,w1"); __asm__ volatile ("addc w7,#0,w0"); __asm__ volatile ("pop.d w4");// ; restore regs __asm__ volatile ("pop.d w6");// __asm__ volatile ("pop.d w8");// return(A); } //#ifdef USE_DWORD_VECTORS /*unsigned long SqRtDWVectors(long long *pdwArgument, unsigned long *pdwAprox) { long NextAproxValue = *pdwAprox; long Difference; int iShift=64; int iShiftdif; register int iWork; int *ptrInt; volatile long long qlTemp; if (NextAproxValue == 0) { ptrInt = ((int*)pdwArgument)+3; // TBD - find MSB => divide by 2 that number and shift 1 == this is a first aproximation value while((iShift&0xf) ==0) { //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift == 0) goto END_DONE; } NextAproxValue = ((long)1)<<(iShift>>1); } //while (NextAproxValue += ((signed long)((*pdwArgument) - (long long)(NextAproxValue * NextAproxValue))/NextAproxValue)>>1) do { qlTemp = ((*pdwArgument) - (long long)((long long)NextAproxValue * (long long)NextAproxValue)); qlTemp /=NextAproxValue; qlTemp >>=1; NextAproxValue +=qlTemp; } while(qlTemp); END_DONE: return NextAproxValue; } */ unsigned long SqRt(long long *pdwArgument, unsigned long *pdwAprox) { volatile long NextAproxValue = *pdwAprox; volatile unsigned int divisor; volatile unsigned int remainder; volatile long Difference; volatile int iShift=64; int iShiftdif; register int iWork; int *ptrInt; if (NextAproxValue == 0) { ptrInt = ((int*)pdwArgument)+3; // TBD - find MSB => divide by 2 that number and shift 1 == this is a first aproximation value while((iShift&0xf) ==0) { //iWork = *ptrInt--; iShift += __builtin_fbcl(*ptrInt--)-1; if (iShift == 0) goto END_DONE; } NextAproxValue = ((unsigned long)1)<<(iShift>>1); } //while (NextAproxValue += ((signed long)((*pdwArgument) - (long long)(NextAproxValue * NextAproxValue))/NextAproxValue)>>1) do { #ifdef __PIC24H__ Difference= ((*pdwArgument) - (long long)(NextAproxValue * NextAproxValue)); if (Difference<0) { if (((unsigned int)NextAproxValue)>(-Difference)) break; Difference=-(long)(__builtin_divmodud((unsigned long)(-Difference), (unsigned int)NextAproxValue,&remainder)); } else { if (((unsigned int)NextAproxValue)> Difference) break; Difference=__builtin_divmodud((unsigned long)Difference, (unsigned int)NextAproxValue,&remainder); } Difference >>=1; #else Difference= ((signed long)((*pdwArgument) - (long long)(NextAproxValue * NextAproxValue))/(signed long)NextAproxValue)>>1; #endif NextAproxValue +=Difference; } while(Difference); END_DONE: return NextAproxValue; } unsigned long SqRtDWVectors(unsigned long long *pdwArgument, unsigned long *pdwAprox) { unsigned long NextAproxValue = *pdwAprox; volatile unsigned long Step; volatile int iShift=64; volatile int iShiftStep; int *ptrInt; volatile unsigned long long qlTemp; unsigned MySign; if ((iShift = findQBitLength(pdwArgument))<=0) { NextAproxValue = 0; goto END_DONE; } if (NextAproxValue == 0) { NextAproxValue = ((long)1)<<(iShift>>1); } //Step = NextAproxValue>>2; //MySign = 0; //whnile (NextAproxValue += ((signed long)((*pdwArgument) - (long long)(NextAproxValue * NextAproxValue))/NextAproxValue)>>1) DO_CHECK_AGAIN: { #ifdef __PIC24H__ qlTemp = (*pdwArgument) - (unsigned long long)muldw_dw((unsigned long)NextAproxValue,(unsigned long)NextAproxValue); #else qlTemp = ((*pdwArgument) - (unsigned long long)((long long)NextAproxValue * (long long)NextAproxValue)); #endif iShiftStep = findQBitLength(&qlTemp); if ((iShift-iShiftStep) < 0) Step= NextAproxValue>>1; else Step= (NextAproxValue>>((iShift-iShiftStep))+1); if (Step == 0) goto END_DONE; if (*(((unsigned int *)&qlTemp)+3) &0x8000) NextAproxValue -=Step; else NextAproxValue +=Step; } if (Step > 1) goto DO_CHECK_AGAIN; END_DONE: return NextAproxValue; } #define DW_MIDDLE Square.dwMiddle #define OFFSET_SIN OffsetSin.iValHH #define OFFSET_SIN_SMALL OffsetSin.bValH #define OFFSET_COS OffsetCos.iValHH #define FLASH_ADDR FlashAddr.dwVal void PROC_2_BYTES(void) { CS_LOW; // open FLASH memory SendSSByteFAST(0x03); // "read FLASH memory" SendSSByte((unsigned char)FlashAddr.iValHH); // address first byte == 1 for second GYRO for 1M memory it is 0x0F0000 SendSSByte(FlashAddr.bValH); // address second byte SendSSByte(FlashAddr.bValL); // address third byte OffsetSin.bValL = (unsigned long) GetSSByte(); OffsetCos.lVal = (long)(signed char)GetSSByte(); OffsetCos.lVal = 0x00000000 - (OffsetSin.lVal * 6 - OffsetCos.lVal); OffsetSin.lVal = 0x80000000 - OffsetSin.lVal; CS_HIGH; // close FLASH memory } /* void PROC_4_BYTES(void) { CS_LOW; // open FLASH memory SendSSByteFAST(0x03); // "read FLASH memory" SendSSByte((unsigned char)FlashAddr.iValHH); // address first byte == 1 for second GYRO for 1M memory it is 0x0F0000 SendSSByte(FlashAddr.bValH); // address second byte SendSSByte(FlashAddr.bValL); // address third byte OffsetSin.bValL = (unsigned long) GetSSByte(); OffsetSin.bValH = (unsigned long) GetSSByte(); OffsetCos.bValL = (unsigned long) GetSSByte(); OffsetCos.bValH = (unsigned long) GetSSByte(); OffsetCos.lVal = 0x00000000 - (OffsetSin.lVal * 6 - OffsetCos.lVal); OffsetSin.lVal = 0x80000000 - OffsetSin.lVal; CS_HIGH; // close FLASH memory }*/ void GetSinCos(void) { // SamplesPerSec =250; OffsetCos.lVal =0; OffsetSin.lVal = 0; // initial clean Square.dwMiddle>>=4; // inital shift = 28 bits FlashAddr.dwVal = 0x50000; // offset in FLASH if (Square.dwMiddle < 0x01d9e) //angle less then 0.096918 x 250=24.229476 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x03b3b) //angle less then 0.137058 x 250=34.264524 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x058d8) //angle less then 0.167859 x 250=41.964839 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x07676) //angle less then 0.193829 x 250=48.457355 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x09413) //angle less then 0.216706 x 250=54.176612 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x0b1b0) //angle less then 0.237389 x 250=59.347244 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x0cf4d) //angle less then 0.256409 x 250=64.102149 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x0eceb) //angle less then 0.274114 x 250=68.528483 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x10a88) //angle less then 0.290741 x 250=72.685233 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x12825) //angle less then 0.306467 x 250=76.616795 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x145c3) //angle less then 0.321427 x 250=80.356709 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x16360) //angle less then 0.335719 x 250=83.929678 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x180fd) //angle less then 0.349427 x 250=87.356631 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x19e9a) //angle less then 0.362617 x 250=90.654129 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x1bc38) //angle less then 0.375345 x 250=93.836233 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x1d9d5) //angle less then 0.387654 x 250=96.913511 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x1f772) //angle less then 0.399584 x 250=99.896040 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x21510) //angle less then 0.411170 x 250=102.792443 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x232ad) //angle less then 0.422436 x 250=105.609073 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x2504a) //angle less then 0.433410 x 250=108.352510 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x26de7) //angle less then 0.444113 x 250=111.028179 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x28b85) //angle less then 0.454565 x 250=113.641208 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x2a922) //angle less then 0.464781 x 250=116.195156 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x2c6bf) //angle less then 0.474777 x 250=118.694163 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x2e45d) //angle less then 0.484568 x 250=121.141949 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x301fa) //angle less then 0.494164 x 250=123.540932 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x31f97) //angle less then 0.503577 x 250=125.894209 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x33d34) //angle less then 0.512817 x 250=128.204298 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x35ad2) //angle less then 0.521895 x 250=130.473789 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x3786f) //angle less then 0.530817 x 250=132.704181 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x3960c) //angle less then 0.539591 x 250=134.897701 goto END_OF_TABLE; OffsetSin.bValH++; if (Square.dwMiddle < 0x3b3aa) //angle less then 0.548226 x 250=137.056402 goto END_OF_TABLE; OffsetSin.bValH++; END_OF_TABLE: FlashAddr.dwVal += ((Square.dwMiddle)<<1); PROC_2_BYTES();} // normalized Quaternions multiplication Q1 = Q1 x Q2 void prodQQ(unsigned char *chQ1, unsigned char *chQ2) { long long QXYZ[4]; unsigned int prodSign; unsigned int MSBQ1; unsigned int MSBQ2; int i; int *ptrOffset = &OffsetQ[30]; unsigned char *ptrQXYZ = ((unsigned char*)&QXYZ[3])+8; // MSB traetment: MSB for q only: other (x,y,z) will be take care in normalization - it is huck MSBQ1 = ((struct Quaternion*)chQ1)->MSB; if (bittest(MSBQ1,0)) // q MSBQ1 |= 0b1000010000100001; if (bittest(MSBQ1,1)) // x MSBQ1 |= 0b0100100000010010; if (bittest(MSBQ1,2)) // y MSBQ1 |= 0b0010000110000100; if (bittest(MSBQ1,3)) // y MSBQ1 |= 0b0001001001001000; MSBQ2 = ((struct Quaternion*)chQ2)->MSB * 0x1111; // sequence starts from low left corner (bakward): // 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 => 0100 // // and backward order i.e. q1*z2 , x1*y2 0010 1000 0100 1110 // on last element q1*q2 needs to prodSign = ((struct Quaternion*)chQ1)->Sign; if (bittest(prodSign,0)) // q prodSign|=0b1000010000100000; if (bittest(prodSign,1)) // x prodSign|=0b0100100000010000; if (bittest(prodSign,2)) // y prodSign|=0b0010000110000000; if (bittest(prodSign,3)) // z prodSign|=0b0001001001000000; prodSign ^= (((struct Quaternion*)chQ2)->Sign)*0x1111; //((struct Quaternion*)chQ1)->Sign = prodSign & 0x000f; prodSign ^= 0b0010100001001110; // attemtion oposit order !!! 0b0100 0010 0100 1110 // chQ1 points to Q1.q and chQ2 points to Q2.z chQ2+=offZ2; i = 16; do { if ((i & 0x0003) == 0) // line { ptrQXYZ -=8; *((unsigned long long*)ptrQXYZ) = 0; } if (prodSign & 0x8000) // product & operation is negative (has to be operation minus) { *((unsigned long long*)ptrQXYZ) -= ((unsigned long long)muldw_dw(*(unsigned long *)chQ1,*(unsigned long*)chQ2))>>8; if (MSBQ1 & 0x8000) { if (MSBQ2 & 0x8000) *((unsigned long long*)ptrQXYZ) -=((unsigned long long)(*(unsigned long*)chQ2)| 0x100000000)<<24; else *((unsigned long long*)ptrQXYZ) -=((unsigned long long)(*(unsigned long*)chQ2))<<24; } if (MSBQ2 & 0x8000) { if (MSBQ1 & 0x8000) *((unsigned long long*)ptrQXYZ) -=((unsigned long long)(*(unsigned long*)chQ1)|0x100000000)<<24; else *((unsigned long long*)ptrQXYZ) -=((unsigned long long)(*(unsigned long*)chQ1))<<24; } } else { *((unsigned long long*)ptrQXYZ) += ((unsigned long long)muldw_dw(*(unsigned long *)chQ1,*(unsigned long*)chQ2))>>8; if (MSBQ1 & 0x8000) { if (MSBQ2 & 0x8000) *((unsigned long long*)ptrQXYZ) +=((unsigned long long)(*(unsigned long*)chQ2)|0x100000000)<<24; else *((unsigned long long*)ptrQXYZ) +=((unsigned long long)(*(unsigned long*)chQ2))<<24; } if (MSBQ2 & 0x8000) { if (MSBQ1 & 0x8000) *((unsigned long long*)ptrQXYZ) +=((unsigned long long)(*(unsigned long*)chQ1)|0x100000000)<<24; else *((unsigned long long*)ptrQXYZ) +=((unsigned long long)(*(unsigned long*)chQ1))<<24; } } chQ1 += *ptrOffset; chQ2 += *(ptrOffset+1); MSBQ1<<=1; MSBQ2<<=1; prodSign<<=1; ptrOffset-=2; } while(--i); // fast implementation // last will be Q1*Q2 // chQ1 is perfectly adjusted prodSign = 0; MSBQ1 = 0; i = 4; ptrQXYZ = ((unsigned char*)&QXYZ[0])+3; do //for (i = 3; i >=0; i--) { if (i != 4) { if (*(ptrQXYZ+4) & 0x80) // negative value of a x,y,z == has to be converted { (*(unsigned long long*)(ptrQXYZ-3)) = (unsigned long long) 0 - (*(unsigned long long*)(ptrQXYZ-3)); prodSign |= 0x10; } } if (*(ptrQXYZ+4) & 0x01) MSBQ1 |=0x10; memcpy((void*)chQ1,(void*)ptrQXYZ,4); ptrQXYZ+=8; chQ1+=4; prodSign>>=1; MSBQ1 >>=1; } while(--i); // this will be sign (*(unsigned int*)chQ1) = prodSign; chQ1+=2; (*(unsigned int*)chQ1) = MSBQ1; } //#ifdef USE_DWORD_VECTORS long SumOfSquareiVectors(int *iX, int *iY, int *iZ) { long long summ; summ = (long)(*iX) * (long)(*iX); summ += (long)(*iY) * (long)(*iY); summ += (long)(*iZ) * (long)(*iZ); return summ; } //#endif long long SumOfSquare(unsigned char *bX, unsigned char *bY, unsigned char *bZ) { long long summ; summ = (*(long*)bX) * (*(long*)bX); summ += (*(long*)bY) * (*(long*)bY); summ += (*(long*)bZ) * (*(long*)bZ); return summ; } unsigned long dwSumOfSquare(unsigned char *bX, unsigned char *bY, unsigned char *bZ) { volatile unsigned long long summ; summ = ((long long)(*(long*)bX)) * ((long long)(*(long*)bX)); summ += ((long long)(*(long*)bY)) * ((long long)(*(long*)bY)); summ += ((long long)(*(long*)bZ)) * ((long long)(*(long*)bZ)); return summ; } #else #define CONVER_TO_INT(x) x #endif #ifdef _Q_PROCESS NormL2iVector(long *lVector, int *iVector) { int iMax = findBitLength(lVector++); int iCur = findBitLength(lVector++); if (iMax < iCur) iMax = iCur; iCur = findBitLength(lVector); if (iMax < iCur) iMax = iCur; iCur = iMax - 15; // max 2 bytes with 1 bit for a sign lVector-=2; if (iCur > 0) { *(iVector++)= (*(lVector++)) >> iCur; *(iVector++)= (*(lVector++)) >> iCur; *(iVector++)= (*(lVector++)) >> iCur; } else { *(iVector++)= *(lVector++); *(iVector++)= *(lVector++); *(iVector++)= *(lVector++); } *iVector = iCur; // iShiftV1/iShiftV2; } NormLVector(long *lVector) { int iMax = findBitLength(lVector++); int iCur = findBitLength(lVector++); if (iMax < iCur) iMax = iCur; iCur = findBitLength(lVector); if (iMax < iCur) iMax = iCur; iCur = iMax - 16; // max 2 bytes with sing already acounted lVector-=2; if (iCur > 0) { *(lVector++) >>= iCur; *(lVector++) >>= iCur; *(lVector++) >>= iCur; } } void ProcessV1(struct Vectors * VV) { NormL2iVector(&VV->V1X, &VV->v1x); VV->V1Sq = SumOfSquareiVectors(&VV->v1x, &VV->v1y, &VV->v1z); } void ProcessV2(struct Vectors * VV, struct Quaternion *QQ) { volatile long long MultSqMV1bySqMV2; volatile unsigned long long qWorkMult; volatile unsigned long InitSqVal; volatile long dwWork; volatile long dwWorkRem; volatile int iShift; volatile unsigned long divident; volatile unsigned int divisor; volatile unsigned int quotient; volatile unsigned int reminder; volatile int iShift2; //unsigned long long qwWord; // any of the vectors can not be bigger then WORD -> othervise (|V1| * |V2|)**2 can be bigger then QWORD // it is kaind limitation - it can be overcome by implementing in dwSumOfSquare avter decimal point WORD addition NormL2iVector(&VV->V2X, &VV->v2x); VV->V2Sq = SumOfSquareiVectors(&VV->v2x, &VV->v2y, &VV->v2z); // V1Sq and V2Sq now less then 30 bits InitSqVal = ((unsigned long)VV->V1Sq + (unsigned long)VV->V2Sq)>>1; // starting aproximation value in iteration for square root calculation // setting that value to Zero forced to calculate "aproxymation" value MultSqMV1bySqMV2 = (unsigned long long)VV->V1Sq * (unsigned long long)VV->V2Sq; VV->V2Sq = SqRtDWVectors(&MultSqMV1bySqMV2, &InitSqVal); VV->V1xV2 = (long)VV->v1x * (long)VV->v2x + (long)VV->v1y * (long)VV->v2y + (long)VV->v1z * (long)VV->v2z; VV->SignV1pV2 = 0; if (*(((unsigned int*)&VV->V1xV2)+1) & 0x8000) // negative values for scalar multiplication => Sin and cos has to be swaped { VV->V1xV2 = -VV->V1xV2; VV->SignV1xV2 = 1; } else VV->SignV1xV2 = 0; // calculatin vector of rotation (vector multiplication btw V1 and V2) // this vetor is perpendicular to plane careated by V1 and V2 VV->V1pV2X = (long)VV->v1y * (long)VV->v2z - (long)VV->v1z * (long)VV->v2y; VV->V1pV2Y = (long)VV->v1z * (long)VV->v2x - (long)VV->v1x * (long)VV->v2z; VV->V1pV2Z = (long)VV->v1x * (long)VV->v2y - (long)VV->v1y * (long)VV->v2x; QQ->Sign = 0; if (*(((unsigned int*)&VV->V1pV2X)+1) & 0x8000) // negative values for scalar multiplication => Sin and cos has to be swaped { VV->V1pV2X = -VV->V1pV2X; VV->SignV1pV2X = 1; QQ->Sx = 1; } else VV->SignV1pV2X = 0; if (*(((unsigned int*)&VV->V1pV2Y)+1) & 0x8000) // negative values for scalar multiplication => Sin and cos has to be swaped { VV->V1pV2Y = -VV->V1pV2Y; VV->SignV1pV2Y = 1; QQ->Sy = 1; } else VV->SignV1pV2Y = 0; if (*(((unsigned int*)&VV->V1pV2Z)+1) & 0x8000) // negative values for scalar multiplication => Sin and cos has to be swaped { VV->V1pV2Z = -VV->V1pV2Z; VV->SignV1pV2Z = 1; QQ->Sz = 1; } else VV->SignV1pV2Z = 0; // again limitation to overcome that problem needs to implement proper devision+reminder // algorithm (probably Knut) NormLVector(&VV->V1pV2X); //VV->V1pV2Sq = SumOfSquareiVectors(&VV->V1pV2X, &VV->V1pV2Y, &VV->V1pV2Z); VV->V1pV2Sq = *((unsigned long*)(&VV->V1pV2X)) * *(unsigned long*)(&VV->V1pV2X); VV->V1pV2Sq +=*((unsigned long*)(&VV->V1pV2Y)) * *(unsigned long*)(&VV->V1pV2Y); VV->V1pV2Sq +=*((unsigned long*)(&VV->V1pV2Z)) * *(unsigned long*)(&VV->V1pV2Z); //VV->V1pV2Sq = (unsigned long long)(*(unsigned long*)(&VV->V1pV2X)) + // (unsigned long long)(*(unsigned long*)(&VV->V1pV2Y)) + // (unsigned long long)(*(unsigned long*)(&VV->V1pV2Z)); // cos(a/2) = sqrt[ (V1xV2/ |V1| /|V2| + 1)/ 2 ] // = sqrt [ (V1xV2 + |V1|*|V2|)/ 2 / |V1| / |V2|] = // = sqrt [ (V1xV2 * |V1|* |V2| + (|V1|*|V2)**2) / 2 / (|V1|*|V2|)**2 ] /* qwWord = (VV->V1xV2 * VV->V2Sq + MultSqMV1bySqMV2)>>1; InitSqVal = VV->V1xV2; qwWord = SqRt(&qwWord, &InitSqVal); qwWord <<=24; VV->CosAdiv2 = qwWord / VV->V2Sq; */ // cos(a/2) = sqrt[ (V1xV2/ (|V1|*|V2|) + 1)/ 2 ] // cos(a/2) * 0x10000 = sqrt[ 0x100000000 * (V1xV2/ (|V1|*|V2|) + 1)/ 2 ] = // sqrt[ 0x80000000 * (V1xV2/ (|V1|*|V2|) + 1) ] = // sqrt[ (0x80000000 * V1xV2/ (|V1|*|V2|) + 0x80000000 ) ] = // sqrt[ ((0x8000 * V1xV2/ (|V1|*|V2|))*0x10000 + 0x80000000 ) ] = // sqrt[ ((0x10000 * V1xV2/ (|V1|*|V2|) + 0x10000)*0x8000 ) ] = // sqrt[ ((0x10000 * V1xV2/ (|V1|*|V2|) + 0x10000)*0x8000 ) ] = // w1 = VV->V1xV2 << 15 / VV->V2Sq // w2 = w1 << 16 + 0x100000000; == (w1+0x10000)<< 16 // cos(a/2) = sqrt(w2) //if (VV->V1xV2 < VV->V2Sq) // is it everytime : scalar multiply less then sqrt(|V1|*|V2|) { iShift = findBitLength(&VV->V2Sq); if (iShift > 16) // value of the diviser bigger then 0xffff => needs to shift both divident and diviser, for quotient it does no matter { if ((iShift-32)<0) divident = VV->V1xV2 << (32-iShift); else divident = VV->V1xV2 >> (iShift - 32); divisor = VV->V2Sq >> (iShift - 16); } else { divident = VV->V1xV2<<16; divisor = VV->V2Sq; } //dwWorkRem = VV->V1xV2 % VV->V2Sq; // TBD !!!! attention needs to calculate simitinues shift to get VV->V2Sq in a range of WORD!!!!! // for test it is OK but for any another cases needs to make shure -- that code will work for modules of a vectors < 240 // basically if two value VV->V1xV2 and VV->V2Sq is close #ifdef __PIC24H__ InitSqVal = __builtin_divmodud(divident, divisor,&reminder); dwWorkRem = __builtin_divmodud(((unsigned long)reminder)<< 16, divisor,&reminder); reminder = __builtin_divmodud(((unsigned long)reminder)<<16, divisor,&reminder); #else InitSqVal = divident /diviser; dwWorkRem = divident % diviser; dwWorkRem = dwWorkRem << 16; dwWorkRem = dwWorkRem/diviser; #endif } if (VV->SignV1xV2) // swapping sin and cos if V1xV2 is negative // is it true? { dwWork = 0x10000 - InitSqVal; MultSqMV1bySqMV2 = dwWork; MultSqMV1bySqMV2 <<=15+16; MultSqMV1bySqMV2 -= (((unsigned long long)dwWorkRem)<<15) + (unsigned long long)(reminder>>1);//dwWorkRem>>1; VV->CosAdiv2 = SqRtDWVectors(&MultSqMV1bySqMV2, &InitSqVal);//<<8; // cos(a/2) with less presision that Quaternion will be used for correction dwWork = 0x10000 + InitSqVal; MultSqMV1bySqMV2 = dwWork; MultSqMV1bySqMV2 <<=15+16; MultSqMV1bySqMV2 += (((unsigned long long)dwWorkRem)<<15) + (unsigned long long)(reminder>>1);//dwWorkRem>>1; } else { dwWork = 0x10000 + InitSqVal; MultSqMV1bySqMV2 = dwWork; MultSqMV1bySqMV2 <<=15+16; MultSqMV1bySqMV2 += (((unsigned long long)dwWorkRem)<<15) + (unsigned long long)(reminder>>1);//dwWorkRem>>1; //InitSqVal = 0;//dwWork;//0xfff0; // small angles VV->CosAdiv2 = SqRtDWVectors(&MultSqMV1bySqMV2, &InitSqVal);//<<8; // cos(a/2) with less presision that Quaternion will be used for correction //QQ->q = // sin(a/2) = sqrt[ (1 - V1xV2/ (|V1|*|V2|))/ 2 ] dwWork = 0x10000 - InitSqVal; MultSqMV1bySqMV2 = dwWork; MultSqMV1bySqMV2 <<=15+16; MultSqMV1bySqMV2 -= (((unsigned long long)dwWorkRem)<<15) + (unsigned long long)(reminder>>1);//dwWorkRem>>1; } //InitSqVal = 0; // VV->SinAdiv2 = SqRtDWVectors(&MultSqMV1bySqMV2, &InitSqVal)<<8; QQ->q = VV->CosAdiv2; iShift = findQBitLength(&VV->V1pV2Sq); iShift2 = findQBitLength(&MultSqMV1bySqMV2); #define MAGIC_001 0x0a9d9227 #define MAGIC_002 0x00002bd6 #define MAGIC_003 0x00003c73 //qWorkMult = (*((unsigned long long*)&MultSqMV1bySqMV2))* (unsigned long long)0x0a9d9227;//<< 32;//(32 - (iShift -iShift2)); (*(((unsigned long long*)&MultSqMV1bySqMV2))) *= (unsigned long long)MAGIC_002;//<< 32;//(32 - (iShift -iShift2)); (*(((unsigned long long*)&MultSqMV1bySqMV2))) /= VV->V1pV2Sq; // slowest operation in all calculation -> needs to use Knut (*(((unsigned long long*)&MultSqMV1bySqMV2))) *= (unsigned long)MAGIC_003; InitSqVal = 0; MultSqMV1bySqMV2 = SqRtDWVectors(&MultSqMV1bySqMV2, &InitSqVal); QQ->x = ((unsigned long long)MultSqMV1bySqMV2 * (unsigned long long) VV->V1pV2X)>>16;// )0x0a9d9227)>>32; // 230*230*180*180/pi/pi QQ->y = ((unsigned long long)MultSqMV1bySqMV2 * (unsigned long long) VV->V1pV2Y)>>16; QQ->z = ((unsigned long long)MultSqMV1bySqMV2 * (unsigned long long) VV->V1pV2Z)>>16; } void GetCorrection(void) { ValCorrect.bValL = GetSSByte(); // it will read X - Y - Z calibration ValCorrect.bValH = GetSSByte(); // in case of two gyro running for a second gyro calbration is Y - X - Z ValCorrect.iValHH = CONVER_TO_INT(GetSSByte()); //ValCorrect.lVal = 0; // correct } //void GetSinCos1(void) //{ // unsigned char Addr0; // unsigned char Addr1; // unsigned char Addr2; // CS_LOW; // open FLASH memory // SendSSByteFAST(0x03); // "read FLASH memory" // SendSSByteFAST(3); // address first byte == 1 for second GYRO for 1M memory it is 0x0F0000 // SendSSByte(0); // address second byte // SendSSByte(0); // address third byte // CS_HIGH; // close FLASH memory //} #endif void Normalize(void) { volatile unsigned long divident; volatile struct MyQWord product; //if (Qgyro.q == 0) //{ // Qgyro.q = 0xFFFFFFFF; // Qgyro.x = 0; // Qgyro.y = 0; // Qgyro.z = 0; // Qgyro.Sign = 0; // //Qgyro.Sign ^= 0b0000000000001110; //} Square.qVal =muldw_dw(Qgyro.q>>2,Qgyro.q>>2); Square.qVal +=muldw_dw(Qgyro.x>>2,Qgyro.x>>2); Square.qVal +=muldw_dw(Qgyro.y>>2,Qgyro.y>>2); Square.qVal +=muldw_dw(Qgyro.z>>2,Qgyro.z>>2); if (Qgyro.MSB) // will be nly MSB for all q,x,y,z normalization should take care { Square.bVal7 |= 0x10; } divident = 0; QgyroM = SqRtDWVectors(&Square.qVal, ÷nt); if ((QgyroM != 0x40000000))// && (QgyroM != 0x3fffffff) && (QgyroM != 0x40000001)) { //Qgyro.MSB = 0; QgyroM = (unsigned long long)0x2000000000000000/(unsigned long long)QgyroM; product.uqVal = muldw_dw(Qgyro.q, QgyroM); if (Qgyro.MSB & 0x01) //q product.dwValH += QgyroM; if (product.wVal3 & 0x8000) Qgyro.MSB |= 0x01; //q Qgyro.q = product.uqVal>>31; product.uqVal = muldw_dw(Qgyro.x, QgyroM); if (Qgyro.MSB & 0x02) //x product.dwValH += QgyroM; if (product.wVal3 & 0x8000) Qgyro.MSB |= 0x02; //x Qgyro.x = product.uqVal>>31; product.uqVal = muldw_dw(Qgyro.y, QgyroM); if (Qgyro.MSB & 0x04) //y product.dwValH += QgyroM; if (product.wVal3 & 0x8000) Qgyro.MSB |= 0x04; //4 Qgyro.y = product.uqVal>>31; product.uqVal = muldw_dw(Qgyro.z, QgyroM); if (Qgyro.MSB & 0x08) //z product.dwValH += QgyroM; if (product.wVal3 & 0x8000) Qgyro.MSB |= 0x08; //x Qgyro.z = product.uqVal>>31; } } void ProcessCMD(unsigned char bByte) { #ifdef _Q_PROCESS unsigned char bWork; struct Readings *ptrReadings; volatile unsigned long divident; volatile unsigned long UpNumber; volatile unsigned int divisor; volatile unsigned int quotient; volatile unsigned int reminder; //struct MyQWord qtemp; #else #ifdef __PIC24H__ char * ptrlFrsCopy; // attention C30 compiled do not understand pointers anything but char* !!!! needs to make a cast!!!! char * ptrlCorrection; #else unsigned char bFrsCopyL; unsigned char bFrsCopyH; unsigned char bFrsCopyHH; #endif unsigned char bCarry; unsigned char bWork; #ifdef __PIC24H__ struct ADJVALUE { unsigned char bWorkL; unsigned char bWorkH; int bWS; } AdjValue __attribute__ ((packed)); register unsigned int W; #else unsigned char bWorkL; unsigned char bWorkH; unsigned char bWS; #endif #endif //unsigned char ptrbWork; //unsigned long int *wWork; //long int wVal; //long int wVal2; // first priority bytes if (AllGyro.GyroDataIs) { //#pragma updateBank 0 AllGyro.GyroDataIs++; if (AllGyro.GyroDataIs == 2) // 0010 AllGyro.TempH = bByte; else if (AllGyro.GyroDataIs == 3) // 0011 AllGyro.TempL = bByte; else if (AllGyro.GyroDataIs == 4) // 0100 { ValCur.iValHH = CONVER_TO_INT(bByte); DataB1.DoIntegrCnt = 0; } else if (AllGyro.GyroDataIs == 5) // 0101 { GD_MAIN_PROC: ValCur.bValH = bByte; // now needs to read flash memory to get calibration values CS_LOW; // open FLASH memory SendSSByteFAST(0x03); // "read FLASH memory" if (DataB1.FirstGyroProcess) SendSSByteFAST(0); // address first byte == 0 else SendSSByteFAST(1); // address first byte == 1 for second GYRO SendSSByte(AllGyro.TempH); // address second byte if (AllGyro.TempL & 0x0f) DataB1.ErrorInTemp = 1; else DataB1.ErrorInTemp = 0; AllGyro.TempL &=0xf0; SendSSByte(AllGyro.TempL); // address third byte if (DataB1.Run2Gyro) // running two gyros in paralel { if (!DataB1.FirstGyroProcess) // 0 = on second gyro fired { ptrReadings = &ValY; PROCESS_LIKE_TWO: ptrReadings->lVal -= ValCur.lVal; GetCorrection(); ptrReadings->lVal -=ValCorrect.lVal; //ptrReadings->lVal >>=1; // make 250 readings per sec } else // 1 = on first gyro fired goto ASSIGN_X; } else // runs one Gyro only { ASSIGN_X: ptrReadings = &ValX; PROCESS_LIKE_ONE: ptrReadings->lVal = ValCur.lVal; GetCorrection(); ptrReadings->lVal +=ValCorrect.lVal; } INTEGRATE: if (!AllGyro.GyroDataIs) CS_HIGH; // close FLASH memory if (DataB1.Run2Gyro) // running two gyros in paralel { if (!DataB1.FirstGyroProcess) // 0 = on second gyro fired { Square.qVal += ((long long)ptrReadings->lVal) * ((long long)ptrReadings->lVal); } else // 1 = on first gyro fired { } if (AllGyro.GyroDataIs == 0) // on processing Z reading { if (!DataB1.FirstGyroProcess) // 0 = on second gyro fired { goto PROCESS_Q; } } } else // runs one Gyro only { //Square.qVal += ((long long)ptrReadings->lVal) * ((long long)ptrReadings->lVal); if (AllGyro.GyroDataIs == 0) // on processing Z reading { PROCESS_Q: // initial 16 sec wait for temperature stabilization NReadings++; //if (DataB1.RstGyroWithVal) //{ // if (NReadings <= 16384)//16384)//8192)//6144)//4096)//2048) // goto SKIP_INITIAL; //} QgyroRead.Sign = 0; if (ValX.bValHHH &0x80) { ValX.lVal = -ValX.lVal; QgyroRead.Sx = 1; } if (ValY.bValHHH &0x80) { ValY.lVal = -ValY.lVal; QgyroRead.Sy = 1; } if (ValZ.bValHHH &0x80) { ValZ.lVal = -ValZ.lVal; QgyroRead.Sz = 1; } Square.qVal =muldw_dw(ValX.dwVal,ValX.dwVal); Square.qVal +=muldw_dw(ValY.dwVal,ValY.dwVal); Square.qVal +=muldw_dw(ValZ.dwVal,ValZ.dwVal); /* // divide by 250*250 == 250*250*4 to get the angle**2 500 is amount of samples per second // and GetSinCos deals with sqaure value divisor = SAMPLES_PER_SEC*SAMPLES_PER_SEC; Square.dwValH = __builtin_divmodud(Square.dwValH, divisor,&reminder); // square cam not be bigger then 6 bytes divident = ((unsigned long)reminder<<16) + (unsigned long)Square.wVal1; Square.wVal1 = __builtin_divmodud(divident, divisor,&reminder); divident = ((unsigned long)reminder<<16) + (unsigned long)Square.wVal0; Square.wVal0 = __builtin_divmodud(divident, divisor,&reminder); */ divident = Square.dwMiddle; //Square.qVal/=10000; //Square.qVal = 264500; //Square.qVal = 17986; //Square.qVal <<=16; //ValX.lVal = 230; // 89 //ValX.lVal = 0x3c15; // 89 //ValY.lVal = 0xf1f; // 2 //ValZ.lVal = 0x1a7; // 1 GetSinCos(); //ValX.lVal = 0x004ff600; // 89 //ValY.lVal = 0x0001cc00; // 2 //ValZ.lVal = 0x0000e600; // 1 //OffsetCos.lVal = 0x00b68c53; //OffsetSin.dwVal= 0x0073819d; //QgyroRead.Sign = 0; if (OffsetCos.dwVal == 0) { QgyroRead.q = 0; QgyroRead.MSB = 1; } else QgyroRead.q = OffsetCos.dwVal; QgyroRead.x = mul3_3(ValX.dwVal,0x517ACD6);//0xA2F59AC);//0x517ACD6);//0x28bd66b); QgyroRead.x = muldw_dw(QgyroRead.x,OffsetSin.dwVal)>>32;//mul3_3(QgyroRead.x,OffsetSin.dwVal); QgyroRead.y = mul3_3(ValY.dwVal,0x517ACD6);//0xA2F59AC);//0x517ACD6);//0x28bd66b); QgyroRead.y = muldw_dw(QgyroRead.y,OffsetSin.dwVal)>>32;//mul3_3(QgyroRead.y,OffsetSin.dwVal); QgyroRead.z = mul3_3(ValZ.dwVal,0x517ACD6);//0xA2F59AC);//0x517ACD6);//0x28bd66b); QgyroRead.z = muldw_dw(QgyroRead.z,OffsetSin.dwVal)>>32;//mul3_3(QgyroRead.z,OffsetSin.dwVal); //Qgyro.q = 0x00ff8345; //Qgyro.x = 0x00023ea2;// 5 = 047e00 //Qgyro.y = 0x00023ea3;// -5 = 047e00 //Qgyro.z = 0; // 0 = 000000 //Qgyro.Sign = 4; // 1 = -q 2=-x 4=-y 8=-z //QgyroRead.q = 0x00b68c53; //QgyroRead.x = 0x002413fd; // 89 = 4ff600 //QgyroRead.y = 0x0000cf8c; // 2 = 01cc00 //QgyroRead.z = 0x000067c6; // 1 = 00e600 //QgyroRead.Sign = 0; //QCorr0 = Qgyro; prodQQ((unsigned char*)&Qgyro,(unsigned char*)&QgyroRead); Normalize(); /*Square.qVal =muldw_dw(Qgyro.q>>2,Qgyro.q>>2); Square.qVal +=muldw_dw(Qgyro.x>>2,Qgyro.x>>2); Square.qVal +=muldw_dw(Qgyro.y>>2,Qgyro.y>>2); Square.qVal +=muldw_dw(Qgyro.z>>2,Qgyro.z>>2); divident = 0; QgyroM = SqRtDWVectors(&Square.qVal, ÷nt); if ((QgyroM != 0x40000000) && (QgyroM != 0x3fffffff)) { QgyroM = (unsigned long long)0x2000000000000000/(unsigned long long)QgyroM; QgyroRead.x = muldw_dw(QgyroRead.x, QgyroM)>>31; QgyroRead.y = muldw_dw(QgyroRead.y, QgyroM)>>31; QgyroRead.z = muldw_dw(QgyroRead.z, QgyroM)>>31; QgyroRead.q = muldw_dw(QgyroRead.q, QgyroM)>>31; }*/ /* if (Square.dwValL != 0x10000) { //if (Square.wVal0 &0x8000) // is it posible ? im mostly cases yes { //divident = ((unsigned long)__builtin_divmodud((unsigned long)0x80000000, Square.wVal0,&reminder))<<9; // shift:16 left, 1 left, and 8 bits right //divident +=__builtin_divmodud(((unsigned long)reminder)<<16, Square.wVal0,&reminder)>>7; // shift: 1 left and 8 bits right divident =((unsigned long)0x80000000/(unsigned long)Square.wVal0)<<9; Square.qVal +=muldw_dw(Qgyro.q,divident); if (Square.bVal7 == 0) { Qgyro.x = mul3_3(Qgyro.x,divident); Qgyro.y = mul3_3(Qgyro.y,divident); Qgyro.z = mul3_3(Qgyro.z,divident); memcpy((void*)&Qgyro.q,(void*)&Square.bVal3,4); if (Qgyro.q == 0) { Qgyro.q = 0xffffffff; } } } //else // needs to shift and counts how many times //{ //} }*/ //prodQQ((unsigned char*)&QgyroRead,(unsigned char*)&Qgyro); //Qgyro = QgyroRead; //Qgyro.x >>=1; //Qgyro.y >>=1; //Qgyro.z >>=1; //Square.qVal = 0x6e35070;// 115560560 //ValCur.dwVal = 0x2a00; // 10752 = (10964 + 10540)/2 //ValCur.dwVal = SqRt(&Square.qVal, &ValCur.dwVal); //Acs.V1X = 3000; //Acs.V1Y = -800; //Acs.V1Z = 10000; //ProcessV1(&Acs); //Acs.V2X = 1000; //Acs.V2Y = 600; //Acs.V2Z = -10200; //ProcessV2(&Acs,&QAcs); //x points left left time= 1 h: //Q=f4de94ac 099fc8ce 4150c87d 22dfc51e m=1.000000 A=33.915012 -0.13 0.87 0.47 //Q=f4769620 07e55f4f 43cd7dd3 21611954 m=1.000000 A=34.533012 -0.10 0.89 0.44 //x points up time=2h //Q=eda5aaa6 2b6a3020 45a19585 303bd57e m=1.000000 A=43.654484 -0.46 0.73 -0.51 //Q=eda6f5e9 2c172390 45779166 2fd4ba9d m=1.000000 A=43.648407 -0.46 0.73 -0.50 if (DataB1.RstGyroWithVal) { if (NReadings%4096 ==0) // first initial calibration { QCorr0 = Qgyro; QCorr0.Sign ^= 0b0000000000001110; prodQQ((unsigned char*)&Qgyro,(unsigned char*)&QCorr0); Normalize(); DataB1.RstGyroWithVal = 0; //Qgyro.q=0xffffffff; //Qgyro.Sign = 0; NReadings = 0; } } else { if (NReadings%256 ==0) { if (CountQGFunc< 16) { QGFunc[CountQGFunc] = Qgyro; QGFunc[CountQGFunc++].Sign ^= 0b0000000000001110; } } if (NReadings%4096 ==0) // needs to corect by initial calibration { NReadings = 0; prodQQ((unsigned char*)&Qgyro,(unsigned char*)&QCorr0); Normalize(); //if ((Qgyro.x > 0xf0000) || (Qgyro.y > 0xf0000) || (Qgyro.z > 0xf0000)) //{ // Square.qVal = ((long long)Qgyro.x) * ((long long)Qgyro.x); // Square.qVal += ((long long)Qgyro.y) * ((long long)Qgyro.y); // Square.qVal += ((long long)Qgyro.z) * ((long long)Qgyro.z); // divident = Square.dwMiddle; // divisor = SAMPLES_PER_SEC*SAMPLES_PER_SEC; // //Square.dwMiddle = (__builtin_divmodud(divident, divisor,&reminder))>>2; // //GetSinCos(); // //Qgyro.q = (OffsetCos.dwVal<<8)|0x000000ff; // //Qgyro.x >>= 8; // //Qgyro.y >>= 8; // //Qgyro.z >>= 8; //} } SKIP_INITIAL:; } } } } else if (AllGyro.GyroDataIs == 6) // 0110 { ValCur.iValHH = CONVER_TO_INT(bByte); } else if (AllGyro.GyroDataIs == 7) // 0111 { ValCur.bValH = bByte; if (DataB1.Run2Gyro) // running two gyros in paralel { if (!DataB1.FirstGyroProcess) // 0 = on second gyro fired { ptrReadings = &ValX; goto PROCESS_LIKE_TWO; } else // 1 = on first gyro fired goto ASSIGN_Y; } else // runs one Gyro only { ASSIGN_Y: ptrReadings = &ValY; goto PROCESS_LIKE_ONE; } goto INTEGRATE; } else if (AllGyro.GyroDataIs == 8) // 1000 { ValCur.iValHH = CONVER_TO_INT(bByte); } else if (AllGyro.GyroDataIs == 9) // 1001 { ValCur.bValH = bByte; AllGyro.GyroDataIs = 0; Main.DoneWithCMD = 1; // long command zttxxyyzz or Zttxxyyzz done if (DataB1.Run2Gyro) // running two gyros in paralel { if (!DataB1.FirstGyroProcess) // 0 = on second gyro fired { ptrReadings = &ValZ; goto PROCESS_LIKE_TWO; } else // 1 = on first gyro fired goto ASSIGN_Z; } else // runs one Gyro only { ASSIGN_Z: ptrReadings = &ValZ; goto PROCESS_LIKE_ONE; } goto INTEGRATE; } return; } #ifdef _Q_PROCESS else if (VAcsOrMagn != NULL && (VAcsOrMagn->DataIs != 0)) { ++VAcsOrMagn->DataIs; if (VAcsOrMagn->DataIs == 2) // X low byte VAcsOrMagn->ReadL = bByte; else if (VAcsOrMagn->DataIs == 3) // X high byte { VAcsOrMagn->ReadH = CONVER_TO_INT(bByte); VAcsOrMagn->XXAve += VAcsOrMagn->XR[VAcsOrMagn->iCount]=VAcsOrMagn->iRead; } else if (VAcsOrMagn->DataIs == 4) // Y low byte VAcsOrMagn->ReadL = bByte; else if (VAcsOrMagn->DataIs == 5) // Y high byte { VAcsOrMagn->ReadH = CONVER_TO_INT(bByte); VAcsOrMagn->YYAve += VAcsOrMagn->YR[VAcsOrMagn->iCount]=VAcsOrMagn->iRead; } else if (VAcsOrMagn->DataIs == 6) // Z low byte VAcsOrMagn->ReadL = bByte; else if (VAcsOrMagn->DataIs == 7) // Z high byte { VAcsOrMagn->ReadH = CONVER_TO_INT(bByte); VAcsOrMagn->ZZAve += VAcsOrMagn->ZR[VAcsOrMagn->iCount]=VAcsOrMagn->iRead; if (++VAcsOrMagn->iCount > MAX_AVER) { VAcsOrMagn->iCount = 0; if (!VAcsOrMagn->TypeSetV1) // initial value V1 was not set yet { VAcsOrMagn->V1X = VAcsOrMagn->XXAve; VAcsOrMagn->V1Y = VAcsOrMagn->YYAve; VAcsOrMagn->V1Z = VAcsOrMagn->ZZAve; VAcsOrMagn->TypeSetV1 = 1; ProcessV1(VAcsOrMagn); } else if (VAcsOrMagn->TypeCalcQ) // needs to calculte Quaternion rotation btw V1 and V2 { if (VAcsOrMagn->TypeDoIntergal) { VAcsOrMagn->XIntergral+= VAcsOrMagn->XXAve - VAcsOrMagn->V2X; VAcsOrMagn->YIntergral+= VAcsOrMagn->YYAve - VAcsOrMagn->V2Y; VAcsOrMagn->ZIntergral+= VAcsOrMagn->ZZAve - VAcsOrMagn->V2Y; } VAcsOrMagn->V2X = VAcsOrMagn->XXAve; VAcsOrMagn->V2Y = VAcsOrMagn->YYAve; VAcsOrMagn->V2Z = VAcsOrMagn->ZZAve; // Quaternion show rotation from V1 -> V2 ProcessV2(VAcsOrMagn,&QAcs); if (VAcsOrMagn->TypeCorectGyro) // needs to correct Gyro readings by Acselerometer or Magnetometr { // this is was was named Kalman filter: // assuming that aselerometr shows correct direction to the center of the earth // (this assumption can be wrong - i.e. on a launched rocket - aceleration will be a superposition btw // vector of trust and gravitation vector) // then rotation from Gyro needs to add to a rotation of a acselerometer // product vector will show difference // then needs to look at the sky, choose some constant, and correction rotation will be // that constant multiplying by differention rotation QCorrection = Qgyro; // needs oposit rotation this is done by inverting vector == minus changed to plus and plus to minus QCorrection.Sign ^= 0b0000000000001110; // sign x,y,z swapped and q stay positive // differential rotation prodQQ((unsigned char*)&QCorrection,(unsigned char*)&QAcs); // now rolling dice : how choose fraction of dirrential rotation // the angle in rotation represented by cos(a/2) // angle will be Const*a with Const=1/128 angla become a/256 or cos become cos(a/256) QCorrection.q = 0x1000000 - ((0x1000000 - QCorrection.q)/256); // the Markov chain or Kalman filter, named it, applying correction prodQQ((unsigned char*)&QCorrection,(unsigned char*)&QAcs); if (VAcsOrMagn->TypeCorectEarth) // needs to correct Gyro by earth rotation (needs both Acselerometer and magnetometr running) { } } } } if (VAcsOrMagn->TypeSetV1) { VAcsOrMagn->XXAve -= VAcsOrMagn->XR[VAcsOrMagn->iCount]; VAcsOrMagn->YYAve -= VAcsOrMagn->YR[VAcsOrMagn->iCount]; VAcsOrMagn->ZZAve -= VAcsOrMagn->ZR[VAcsOrMagn->iCount]; } VAcsOrMagn->DataIs = 0; Main.DoneWithCMD = 1; // long command axxyyzz done } } #endif // Quaternion processing else if (DataB0.Reg15) { DataB0.Reg15 = 0; DataB0.Reg16 = 1; REG15 = bByte; return; } else if (DataB0.Reg16) { DataB0.Reg16 = 0; DataB0.Reg17 = 1; REG16 = bByte; return; } else if (DataB0.Reg17) { DataB0.Reg17 = 0; DataB0.Reg17Second = 1; REG17 = bByte; return; } else if (DataB0.Reg17Second) { DataB0.Reg17Second = 0; REG17Second = bByte; DataB1.ExecInit = 1; Main.DoneWithCMD = 1; return; } ////////////////////////////////////////////////////////////////////// // done area with no difference as a command or stream distingvishing ////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////// if (!Main.getCMD) { ///////////////////////////////////////////////////////////////////// // this is a STREAM area of processing #include "commc3.h" // additional code proceessed as Cmd : if (DataB3.FlashWrite) { if (DataB3.FlashWriteLen) { DataB3.FlashWriteLen = 0; CountWrite = bByte; DataB3.FlashRead = 0; CS_LOW; } else { if (DataB3.FlashRead) { DataB3.FlashRead = 0; if (!Main.ComNotI2C) { //do //{ // InsertI2C(GetSSByte()); // read byte from FLASh will goes to I2C < 10 bytes //} while(--bByte); //InsertI2C('@'); } else { Main.SendWithEsc = 1; do { putchWithESC(GetSSByte()); // read byte from FLASh will goes to Com // if size bigger then 13 bytes it can be delay (putchWithESC waits out queue avalable space) } while(--bByte); Main.SendWithEsc = 0; if (UnitFrom) putch(UnitFrom); } goto DONE_WITH_FLASH; } else if (CountWrite == 1) // this will be last byte to write or it can be symb=@ request to read { if (bByte == '@') // without CS_HIGH will be next read { DataB3.FlashRead = 1; if (!Main.ComNotI2C) // CMD comes from I2C - reply from read should goes back to I2C { //InsertI2C('<'); //InsertI2C(UnitFrom); //if (SendCMD) // InsertI2C(SendCMD); } else // CMD comes from Com == relay (read) must go back to comm { if (UnitFrom) { putch(UnitFrom); if (SendCMD) putch(SendCMD); } } return; } } SendSSByte(bByte); //SendSSByteFAST(bByte); //for testing only if (--CountWrite) return; DONE_WITH_FLASH: DataB3.FlashWrite = 0; CS_HIGH; if (!Main.ComNotI2C) // CMD comes from I2C - reply from read should goes back to I2C { // initiate send using I2C //InitI2cMaster(); } else { //if (UnitFrom) // putch(UnitFrom); } Main.DoneWithCMD = 1; // long command flash manipulation done } return; } #ifndef _16F88 else if (iRstPtr) { switch(++iRstPtr) { case 2:RstType = bByte;break; case 3:RstXXH = bByte; break; case 4:RstXXL = bByte; break; case 5:RstYYH = bByte; break; case 6:RstYYL = bByte; break; case 7:RstZZH = bByte; break; case 8:RstZZL = bByte; #ifdef _Q_PROCESS // C = MIAGgxyz bits - // M - run magnetometr // I - do interation for Acselerometr // A - run Acselerometr // G - run Gyro 1 // g - run Gyro 2 // set values for x, y, or/and z // XX, YY, ZZ is a initial rotational values AllGyro.Hbyte+Lbyte // TBD: instead (or additionaly) of vector needs to do correction by Quaternion if (RstType &0b00010000) // run 1 Gyro { if (RstType &0b00001000) // run 2 Gyro == run two Gyro together { } else // it mean that needs to run only one Gyro goto START_G1; } else if (RstType &0b00001000) // run 2 Gyro goto START_G2; // TBD: this is agly!!! if (RstType &0b00100000) // run Acselerometr { Acs.TypeSetV1 = 0; // Vector1 Was Not Set Acs.TypeAcsOrMagn = 0; // this will be acselerometr Acs.TypeCalcQ = 1; // needs to calculates qunternion Acs.TypeCorectGyro = 1; // needs to do do Qunterion correction if (RstType &0b01000000) // run Acselerometr with ingration Acs.TypeDoIntergal = 0; } NReadings = 0; CountQGFunc = 0; #endif // non quaternion FSR_REGISTER = &ZeroAvDblG; bWork = 2*sizeof(ZeroAvDblG); // zero also ZeroAvDiffG CLEAN_2: PTR_FSR = 0; FSR_REGISTER++; if (--bWork) goto CLEAN_2; iRstPtr = 0; AllGyro.CalibrUnit = 0x68;//0; DataB1.Run2Gyro = 1; DataB1.RstGyroWithVal = 1; DataB1.FirstGyroProcess = 0; Main.DoneWithCMD = 1; // long command kCxxyyzz done goto ZERO_COUNTERS; break; } } #endif //else if (DataB0.TimerPresc) //{ // DataB0.TimerPresc = 0; // DataB0.Timer = 1; // bByte &=0x7; // if (bittest(TIMER0_CONTROL_REG,7)) // bitset(bByte,7); // if (bittest(TIMER0_CONTROL_REG,6)) // bitset(bByte,6); // if (bittest(TIMER0_CONTROL_REG,5)) // bitset(bByte,5); // // TIMER0_CONTROL_REG = bByte; // return; //} //else if (DataB0.Timer) //{ // DataB0.Timer = 0; // ByteTimer = bByte; // return; //} #include "commc4.h" // additional code: // else if (bByte == 'F') // manipulation with FLASH memory: read/write/erase/any flash command { Main.DoneWithCMD = 0; // long command DataB3.FlashWrite = 1; DataB3.FlashWriteLen = 1; // send something to FLASH // F // send and receive responce from FLASH // F@ // in last case must include simbol '@' // F\x01\x06 == write enable (flash command 06) // F\x05\x03\x00\x12\x34@\x04 == read 4 bytes from a address 0x001234 // F\x01\x06F\x0c\x02\x00\x11\x22\x00\x00\x00\x00\x00\x00\x00\x00 == write 8 bytes to address 0x001122 // F\x01\x06F\x04\x20\x00\x04\x00 == erase sector (4K) starting from address 0x000400 } else if (bByte == 'w') // wait time btw steps { SET_WAIT: I2C.Timer0Fired = 0; DataB0.Timer0Waiting = 1; TIMER0_BYTE = ByteTimer; TIMER0_INT_FLG = 0; // clean timer0 interrupt TIMER0_INT_ENBL = 1; // enable timer0 interrupt //bitclr(TIMER0_CONTROL_REG,5); #ifndef __PIC24H__ T0SE = 0; #endif #ifdef _18F2321 T08BIT = 1; // set timer 0 as a byte counter TMR0ON = 1; #endif } else if (bByte == 'I') // init GYRO I { Main.DoneWithCMD = 0; // long command DataB1.GyroRun = 0; DataB0.Reg15 = 1; DataB0.Reg16 = 0; DataB0.Reg17 = 0; DataB0.Reg17Second = 0; } else if (bByte == 'k') // start two gyros command: kCXXYYZZ { // C = EIAGgxyz bits - // M - run magnetometr // I - do interation for Acselerometr // A - run Acselerometr // G - run Gyro 1 // g - run Gyro 2 // set values for x, y, or/and z // XX, YY, ZZ is a initial rotational values AllGyro.Hbyte+Lbyte // TBD: instead (or additionaly) of vector needs to do correction by Quaternion Main.DoneWithCMD = 0; // long command #ifndef _16F88 iRstPtr = 1; #endif //AllGyro.CalibrUnit = 0; //DataB1.Run2Gyro = 1; //goto ZERO_COUNTERS; } else if (bByte == 's') // start gyro 2 { START_G2: AllGyro.CalibrUnit = 0x69; goto ZERO_COUNTERS; } else if (bByte == 'S') // start gyro 1 { START_G1: AllGyro.CalibrUnit = 0x68; ZERO_COUNTERS: #ifdef _Q_PROCESS //memset((void*)&Qgyro,0,sizeof(Qgyro)); QgyroM = 0x3fffffff; Qgyro.q = 0xffffffff; Qgyro.x = 0x00000000; Qgyro.y = 0x00000000; Qgyro.z = 0x00000000; Qgyro.Sign = 0; // all positive #endif // zero integrational counter bWork = sizeof(AllGyro) -1; FSR_REGISTER = (char*)&AllGyro; FSR_REGISTER++; do { PTR_FSR = 0; FSR_REGISTER++; } while(--bWork); //AllGyro.ZZL = 0x20; //AllGyro.YYL = 0x40; //AllGyro.XXL = 0xa0; START_GYRO: DataB1.GyroRun = 1; //GyroGetStatus = 1; // this line for work without interrupr DataB1.GyroStatusIs = 0; DataB1.GyroGetData = 0; AllGyro.GyroDataIs = 0; if (AllGyro.CalibrUnit == 0x69) { START_SECOND_G: Main.ExtInterrupt1 = 1; // first read cicle initiated #ifndef _16F88 INT1IE =1; // enable external interrupt for GYRO 2 #endif #ifdef _Q_PROCESS if (RstType &0b00100000) // run Acselerometr { Main.ExtInterrupt2 = 1; // first read cicle initiated INT2IE =1; // enable external interrupt for acselerometr } #endif } else { Main.ExtInterrupt = 1; // first read cicle initiated INT0_ENBL = 1; // enable external interrupt for GYRO 1 if (AllGyro.CalibrUnit == 0) // request to run two gyros goto START_SECOND_G; } } else if (bByte == 'P') // stop gyro { DataB1.Run2Gyro = 0; DataB1.GyroRun = 0; INT0_ENBL = 0; // disable external interrupt for GYRO 1 #ifndef _16F88 INT1IE = 0; // disable external interrupt got GYRO 2 #endif } //else if (bByte == 'u') // reply from gyro with a status //{ // DataB1.GyroStatusIs = 1; //} #ifdef _Q_PROCESS else if (bByte == 'm') // reply from a Magnetometr with a data:xxyyzz { Magn.TypeAcsOrMagn = 0; VAcsOrMagn = &Magn; goto SAME_AS_ACS; } else if (bByte == 'a') // reply from a Acselerometr with a data:xxyyzz { Acs.TypeAcsOrMagn = 0; VAcsOrMagn = &Acs; SAME_AS_ACS: Main.DoneWithCMD = 0; // long command VAcsOrMagn->DataIs = 1; } #endif else if (bByte == 'z') // reply from gyro 1 with a data:zttxxyyzz tt =temperature xx,yy,zz if Gyro 1 readings { //if (ddebug) //{ // Main.DoneWithCMD = 0; // long command //} //ddebug = 1; Main.DoneWithCMD = 0; // long command AllGyro.GyroDataIs = 1; DataB1.FirstGyroProcess = 1; } else if (bByte == 'Z') // reply from gyro 2 with a data:Zttxxyyzz tt= temprerature xx,yy,zz is Gyro 2 readings { Main.DoneWithCMD = 0; // long command AllGyro.GyroDataIs = 1; DataB1.FirstGyroProcess = 0; } else if (bByte == 'A') // output Acselerometer Readings { QCopy = QAcs; goto DO_SPEED; } else if (bByte == 'M') // output Magnetometr Readings { QCopy = QMagn; goto DO_SPEED; } else if (bByte == 'Q') { // over com - needs to take care about ESC chars // for pic24 it is posible to use DMA but anyway it is backwards compatible QCopy = Qgyro; if (CountQGFunc>=16) { if (!DataB1.RstGyroWithVal) prodQQ((unsigned char*)&QCopy,(unsigned char*)&QGFunc[CountQGFunc>>4]); } DO_SPEED: putch(UnitFrom); putch(SendCMD); #ifdef SPEED_SEND_DATA #ifdef _Q_PROCESS ptrSpeed = &QCopy;//&AllGyro.TempL;//from IntegrCount till AllGyro.TempL (last byte AllGyro.TempL does not transfered) ptrSpeed--; LenSpeed = sizeof(QCopy); #else ptrSpeed = &AllGyro.CalibrUnit;//&AllGyro.TempL;//from IntegrCount till AllGyro.TempL (last byte AllGyro.TempL does not transfered) ptrSpeed--; LenSpeed = sizeof(AllGyro) - sizeof(AllGyro.GyroDataIs) - sizeof(HBYTE) - 1;// 19; // AllGyro LenSpeed++; #endif Speed.SpeedSendLocked = 0; Speed.SpeedSendUnit = 0; Speed.SpeedSendWithESC = 1; Speed.SpeedESCwas = 0; Speed.SpeedSend = 1; // LAST TO INITIATE TXIE = 1; #endif } #ifdef INCLUDE_CALIBR else if (bByte == 'G') // set calibration GYRO 2 then set run == data is not interrated but { AllGyro.CalibrUnit=0x69; goto CALIBRATION_1; } else if (bByte == 'C') // set calibration GYRO 1 then set run == data is not interrated but { AllGyro.CalibrUnit=0x68; CALIBRATION_1: InitCalibr(); DataB1.Calibration = 1; DataB1.SecondCalibr = 0; goto START_GYRO; } else if (bByte == 'c') // stop calibration { AllGyro.CalibrUnit=0; DataB1.Calibration = 0; DataB1.GyroRun = 0; INT0_ENBL = 0; // disable external interrupt for GYRO 1 #ifndef _16F88 INT1IE = 0; // disbalke external interrupt for GYRO2 #endif } #endif SKIP_BYTE:; } // do not confuse: this is a else from getCMD == 1 } #pragma codepage 1 //unsigned char bBy; //if (CMDget) // if this is internal procesing jpeg ? //{ // bBy = InQu[iPtr2InQu]; // function called when Queue is not empty; check next pop byte in queue // if (bBy == UnitADR) // needs to ESC // goto SET_FOR_ESC_ADR; // else if (bBy == '@') // needs to ESC // { //SET_FOR_ESC: // if (!Main.PrepI2C) // { //SET_FOR_ESC_ADR: // Main.ESCNextByte = 1; // } // else // I2C.ESCI2CChar = 1; // } // else if (bBy == '>') // needs to ESC // goto SET_FOR_ESC; // else // { // Main.ESCNextByte = 0; // I2C.ESCI2CChar = 0; // } // jpeg done //return 2; unsigned char CallBkComm(void) // return 1 == process queue; 0 == do not process; // 2 = do not process and finish process 3 == process and finish internal process { // in case 0 fucntion needs to pop queue byte by itself unsigned char bReturn = 0; if (Main.ComNotI2C) // prev was COM processing { //Main.ComNotI2C = 1; bReturn = 1; // do process data } else // prev was I2C processing { if (Main.DoneWithCMD) { Main.ComNotI2C = 1; bReturn = 1; // do process data } } return bReturn; } unsigned char CallBkI2C(void)// return 1 == process queue; 0 == do not process; // 2 = do not process and finish process 3 == process and finish internal process { // in case 0 fucntion needs to pop queue byte by itself unsigned char bReturn = 0; if (Main.ComNotI2C) // prev was COM processing { if (Main.DoneWithCMD) // it is CMD processing now continue end of a CMD from com { Main.ComNotI2C = 0; bReturn = 1; // do process data } } else // prev was I2C data processing { //Main.ComNotI2C = 0; bReturn = 1; // do process data } return bReturn; } unsigned char CallBkMain(void) // 0 = do continue; 1 = process queues { unsigned char bWork; RtccReadTimeDate(&RtccTimeDateVal); //if (DataB0.Timer0Waiting) //{ // if (I2C.Timer0Fired) // DataB0.Timer0Waiting = 0; // else // return 0; //} //else if (DataB1.GetGyroWait) //{ // if (I2C.Timer0Fired) // { // DataB1.GetGyroWait = 0; // //DataB1.GyroGetStatus = 1; // } // return 1; //} //else if (INT0_ENBL) // can be two cases INT0_ENBL = 1 && INT1IE = 0 for a case when one from two gyro running { // INT0_ENBL = 1 && INT1IE = 1 both gyro running if (DataB1.Run2Gyro) // running two gyros in paralel { if (I2C_B1.I2CMasterDone) { #ifdef _Q_PROCESS if (INT2IE) { if (Main.ExtInterrupt2) // fired inrrupt by acselerometr { Main.ExtInterrupt2 = 0; putchI2C(0x32); // register to read == starts from x I2Caddr= 0x53; LenI2CRead = 6; // amount bytes to read from I2C I2C_B1.I2Cread = 0; InsertI2C('a'); InitI2cMaster(); return 1; } } #endif if (Main.ExtInterrupt) goto DONE_INT1; #ifndef _16F88 if (DataB1.FirstGyroProcess == 1) goto DONE_INT2; #endif } } CHECH_1_INT: if (Main.ExtInterrupt) { if (I2C_B1.I2CMasterDone) { DONE_INT1: Main.ExtInterrupt = 0; AllGyro.CalibrUnit = 0x68; goto GET_GYRO_DATA; // and directly process interrupt } } } #ifndef _16F88 else if (INT1IE) { CHECH_2_INT: if (Main.ExtInterrupt1) { if (I2C_B1.I2CMasterDone) { DONE_INT2: Main.ExtInterrupt1 = 0; AllGyro.CalibrUnit = 0x69; goto GET_GYRO_DATA; // and directly process interrupt } } } #endif else if (AOutI2CQu.iQueueSize == 0) // I2C out queue empty { //if (AInQu.iQueueSize == 0) // in Com queue empty { //if (!getCMD) // and no command processing { if (!I2C.LockToI2C) { if (DataB1.GyroRun) { if (DataB1.GyroGetStatus) { //// send I2C Gyro directly == can not wait ////Main.ExtInterrupt = 0; ////Main.ExtInterrupt1 = 0; //DataB1.GyroGetStatus = 0; //putchI2C(0x1a); // status register //DataB1.GyroStatusIs = 1; //bWork = 1; // amount bytes to read from I2C GET_GYRO_DATA: #ifdef I2C_INT_SUPPORT #else DataB1.GyroGetData = 0; AllGyro.GyroDataIs = 1; #endif //putch(0x1d); // register to read starts skipping temperature putchI2C(0x1b); // register to read == starts from temperature bWork = 8; // amount bytes to read from I2C START_I2C_B: I2Caddr= AllGyro.CalibrUnit; //if (AllGyro.CalibrUnit == 0x68) // DataB1.FirstGyroProcess =1; //else // DataB1.FirstGyroProcess =0; LenI2CRead = bWork; I2C_B1.I2Cread = 0; // bit == 0 mean it starts with write operation #ifdef I2C_INT_SUPPORT //////////////////////////////////////////////////////////////////////////////////////////////// #ifdef INCLUDE_CALIBR if (DataB1.Calibration) // store 16 sec calibration to FLASH and output { InitI2cMaster(); WAIT_I2C_DONE_C: if (!I2C_B1.I2CMasterDone) // wait ed of I2C operation goto WAIT_I2C_DONE_C; if (ProcessCalibr()) // 16 seconds done == stop calibration { AllGyro.CalibrUnit=0; DataB1.Calibration = 0; DataB1.GyroRun = 0; INT0_ENBL = 0; // disable external interrupt for GYRO 1 INT1IE = 0; // disbalke external interrupt for GYRO2 } AllGyro.GyroDataIs = 0; DataB1.GyroStatusIs = 0; } else { #endif // INCLUDE_CALUBR if (AllGyro.CalibrUnit == 0x68)//DataB1.FirstGyroProcess) InsertI2C('z');// I2CReplyCMD = 'z'; else InsertI2C('Z');;// I2CReplyCMD = 'Z'; //I2CReplyCMD = 'z'; //I2C.RetransI2CCom = 0; //I2C.I2CReplyExpected = 1; InitI2cMaster(); // and continue process - needs to catch data from I2C input queue //Main.PrepI2C = 0; #ifdef INCLUDE_CALIBR } #endif #else // not I2C_MASTR_SUPPORT /////////////////////////////////////////////////////////////////////////////////// B_I2C_MSG_WAIT: if (InitI2cMaster()) // TBD I2C line busy by somebody else what to do? goto B_I2C_MSG_WAIT; sendI2C(); //if (sendI2C()) // goto B_I2C_RELESE_I2C; I2C.NextI2CRead =1; // it will be sertart conditions ReleseI2cMaster(); I2C_B1.I2Cread = 1; //' bit == 1 mean it is write operation B_I2C_WAIT_READ: // TBD this loop has to have limitation - bus can be dead if (InitI2cMaster()) { goto B_I2C_RESTART_COLIS; } sendI2C(); //if (sendI2C()) // send address only - TBD needs to check how was ACK on address // goto B_I2C_RELESE_I2C; receiveI2C(); B_I2C_RESTART_COLIS: B_I2C_RELESE_I2C: I2C.NextI2CRead = 0; ReleseI2cMaster(); #ifdef INCLUDE_CALIBR if (DataB1.Calibration) // store 2 calibration reading and output { if (ProcessCalibr()) // 16 seconds done == stop calibration { AllGyro.CalibrUnit=0; DataB1.Calibration = 0; DataB1.GyroRun = 0; INT0_ENBL = 0; // disable external interrupt for GYRO 1 #ifndef _16F88 INT1IE = 0; // disbalke external interrupt for GYRO2 #endif } } else #endif { while(AInI2CQu.iQueueSize) { //ddebug = 1; ProcessCMD(getchI2C()); // process all responce == // it is OK - it process with GyroStatusIs or AllGyro.GyroDataIs // in first priority }; } AllGyro.GyroDataIs = 0; DataB1.GyroStatusIs = 0; #endif // not I2C_INT_SUPPORT ///////////////////////////////////////////////////////////////////////////////// } } else if (DataB1.ExecInit) { if (!Main.getCMD) // and no processing in command mode { //bitset(PORTA,4); DataB1.ExecInit = 0; I2Caddr = 0x68; //BlockComm = 1; SEND_INIT_TOGYROS: Main.PrepI2C = 1; // set out queue to I2C // start from register 0x15: putchI2C(0x15); // reg 0x15 // Fsample = Finternal / (divider+1), where Finternal is either 1kHz or 8kHz putchI2C(REG15); // 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 putchI2C(REG16); // 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); if (I2Caddr == 0x68) putchI2C(REG17); else putchI2C(REG17Second); //putchI2C(0b10010101); //I2CReplyCMD = UnitADR; //I2C.I2CReplyExpected = 1; I2C.WaitQuToEmp = 0; I2C_B1.I2Cread = 0; I2C.NextI2CRead = 0; //ddebug = 2; Main.getCMD = 1; // this framing will force processCMD to finish I2C in one chank ProcessCMD('@'); // send over I2C == this will wait end of I2C communication Main.getCMD = 0; // other way to do this is // now second device =0x69 if (I2Caddr == 0x68) { I2Caddr = 0x69; goto SEND_INIT_TOGYROS; } } } } } } } return 1; } void Reset_device(void) { #ifdef __PIC24H__ // Configure Oscillator to operate the device at 40Mhz // Fosc= Fin*M/(N1*N2), Fcy=Fosc/2 // Fosc= 8M*40/(2*2)=80Mhz for 8M input clock PLLFBD=42; // 42// M=40 // for value = 50 it can be 92.125MHz and 46MIPS needs to make shure that FIN will be more then 4MHz and less 8MHz // OCTUN set more then 8MHz can be wrong CLKDIVbits.PLLPOST=0; // N2=2 PLL VCO Output Divider Select bits (also denoted as ‘N2’, PLL postscaler) // 00 = Output/2 CLKDIVbits.PLLPRE=0; // N1=2 PLL Phase Detector Input Divider bits (also denoted as ‘N1’, PLL prescaler) // 00000 = Input/2 (default) OSCTUN=0;//0x14; // Tune FRC oscillator, if FRC is used // bit 5-0 TUN<5:0>: FRC Oscillator Tuning bits(1) // 111111 = Center frequency -0.375% (7.345 MHz) // 100001 = Center frequency -11.625% (6.52 MHz) // 100000 = Center frequency -12% (6.49 MHz) // 011111 = Center frequency +11.625% (8.23 MHz) // 011110 = Center frequency +11.25% (8.20 MHz) // 000001 = Center frequency +0.375% (7.40 MHz) // 000000 = Center frequency (7.37 MHz nominal) // 010100 = 8MHz RCONbits.SWDTEN=0; // Disable Watch Dog Timer ??? // Clock switch to incorporate PLL __builtin_write_OSCCONH(0x01); // Initiate Clock Switch to Primary // Oscillator with PLL (NOSC=0b011) __builtin_write_OSCCONL(0x01); // Start clock switching // bit 7 CLKLOCK: Clock Lock Enable bit 0 = Clock switching is enabled, system clock source can be modified by clock switching // bit 6 IOLOCK: Peripheral Pin Select Lock bit 0 = Peripherial pin select is not locked, write to peripheral pin select registers allowed // bit 5 LOCK: PLL Lock Status bit (read-only) // bit 4 Unimplemented: Read as ‘0’ // bit 3 CF: Clock Fail Detect bit (read/clear by application) // bit 2 Unimplemented: Read as ‘0’ // bit 1 LPOSCEN: Secondary (LP) Oscillator Enable bit 0 = Disable secondary oscillator // bit 0 OSWEN: Oscillator Switch Enable bit 1 = Request oscillator switch to selection specified by NOSC<2:0> bits while (OSCCONbits.COSC != 0b001); // Wait for Clock switch to occur check for: // 011 = Primary oscillator (XT, HS, EC) with PLL while(OSCCONbits.LOCK!=1) {}; // Wait for PLL to lock // bit 5 LOCK: PLL Lock Status bit (read-only) // 1 = Indicates that PLL is in lock, or PLL start-up timer is satisfied // 0 = Indicates that PLL is out of lock, start-up timer is in progress or PLL is disabled OSCCONbits.LPOSCEN = 1; //__asm__ volatile ("mov #NVMKEY,w1"); //__asm__ volatile ("mov #0x55,w2"); //__asm__ volatile ("mov #0xaa,w3"); //__asm__ volatile ("mov w2,[w1]"); //__asm__ volatile ("mov w3,[w1]"); //__asm__ volatile ("bset RCFGCAL,#13"); // //RCFGCALbits.RTCWREN = 1; //nop(); //__builtin_write_RTCWEN(); //RCFGCALbits.RTCEN = 1; //RCFGCALbits.RTCSYNC = 0; //RCFGCALbits.RTCOE = 0; // disable analog AD1CON1bits.ADON = 0; // and switch analog pins to digital AD1PCFGL = 0xffff; //AD1PCFGH = 0xffff; // porta is not re-mappable and on 502 device it is RA0-RA4 // SPI output in FLASH mem terminoligy: // SSCLOCK RA0(pin2), SSDATA_IN RA1(pin3), SSDATA_OUT RA2(pin9), SSCS RA3(pin10) // 0 0 IN 1 // RA4 secondary oscilator TRISA = 0b00010100; //0 = Output, 1 = Input PORTA = 0b00001000; // RB4 = secondary oscliator TRISBbits.TRISB4 = 1; // this kaind funny, and for PIC24 and up = PRX pins can be reassigned to differrent preferias // additionaly needs to remember on which pin sits which PRX : // VSIAK SVERCHOK ZNAI SVOI SHESTOK __builtin_write_OSCCONL(OSCCON & 0xbf); // unlock port remapping // INT0 == pin 16 // INT1 == pin 21 == PR10 #ifdef HI_TECH_C #else IN_FN_PPS_INT1 = IN_PIN_PPS_RP10; // RPINR0 // INT2 == pin 22 == PR11 IN_FN_PPS_INT2 = IN_PIN_PPS_RP11; // PR5 - Serial RX Pin 14 IN_FN_PPS_U1RX = IN_PIN_PPS_RP5; // RR6 - Serial TX Pin 15 OUT_PIN_PPS_RP6 = OUT_FN_PPS_U1TX; #endif // I2C: // SCL1 = I2C clock Pin 17 (this is NOT alernative I2c set as FPOR = 1 in configuration) // SDA1 = I2C data Pin 18 this two pins permamet __builtin_write_OSCCONL(OSCCON | 0x40); //lock back port remapping //RBPU_ = 0; //bitclr(OPTION,RBPU_); //Each of the PORTB pins has a weak internal pull-up. A //single control bit can turn on all the pull-ups. This is //performed by clearing bit RBPU (OPTION_REG<7>). //The weak pull-up is automatically turned off when the //port pin is configured as an output. The pull-ups are //disabled on a Power-on Reset. INT0_ENBL = 0; // disable external interrupt for GYRO 1 INT1IE = 0; // disable external interrupt for GYRO2 enable_uart(); //Setup the hardware UART for 20MHz at 9600bps // next two bits has to be set after all intialization done //PEIE = 1; // bit 6 PEIE: Peripheral Interrupt Enable bit // 1 = Enables all unmasked peripheral interrupts // 0 = Disables all peripheral interrupts //GIE = 1; // bit 7 GIE: Global Interrupt Enable bit // 1 = Enables all unmasked interrupts // 0 = Disables all interrupts enable_I2C(); TIMER0_INT_FLG = 0; // clean timer0 interrupt TIMER0_INT_ENBL = 0; // diasable timer0 interrupt TMR1IF = 0; // clean timer0 interrupt TMR1IE = 0; // diasable timer0 interrupt INT0_EDG = 1; // 1 = Interrupt on negative edge INT0_FLG = 0; // clean extrnal interrupt RB0 pin 6 INT1IF = 0; INTEDG1 = 1; INT2IF = 0; INTEDG2 = 1; //T1CONbits.TON = 1; //T1CONbits.TCS = 1; RtccInitClock(); //turn on clock source RtccWrOn(); //enable RTCC peripheral RtccWriteTimeDate(&RtccTimeDate,TRUE); RtccReadTimeDate(&RtccTimeDateVal); mRtccOn(); #else // this is will be better at the begining of a program OSCCON = 0b01110000; //OSCILLATOR CONTROL REGISTER (ADDRESS 8Fh) // bit 6-4 IRCF<2:0>: Internal RC Oscillator Frequency Select bits // 000 = 31.25 kHz // 001 = 125 kHz // 010 = 250 kHz // 011 = 500 kHz // 100 = 1 MHz // 101 = 2 MHz // 110 = 4 MHz // 111 = 8 MHz // bit 3 OSTS: Oscillator Start-up Time-out Status bit(1) // 1 = Device is running from the primary system clock // 0 = Device is running from T1OSC or INTRC as a secondary system clock // Note 1: Bit resets to ‘0’ with Two-Speed Start-up mode and LP, XT or HS selected as the // oscillator mode. // bit 2 IOFS: INTOSC Frequency Stable bit // 1 = Frequency is stable // 0 = Frequency is not stable // bit 1-0 SCS<1:0>: Oscillator Mode Select bits // 00 = Oscillator mode defined by FOSC<2:0> // 01 = T1OSC is used for system clock // 10 = Internal RC is used for system clock // 11 = Reserved #ifdef _NOT_SIMULATOR while((OSCCON&0b00000100) == 0); //Wait for frequency to stabilize #endif #ifdef _18F2321 PLLEN = 1; #ifdef _NOT_SIMULATOR while((OSCCON&0b00000100) == 0); //Wait for frequency to stabilize #endif ADCON0 = 0b00000000; ADCON1 = 0b00001111; // SPI output in FLASH mem terminoligy: // SSCLOCK RA7(pin9), SSDATA_IN RA6(pin10), SSDATA_OUT RA4(pin6), SSCS RA3(pin5) // 0 0 IN 1 TRISA = 0b00010000; //0 = Output, 1 = Input PORTA = 0b00001000; // RB0 - external INT Pin 21 // RB1 - external INT Pin 22 TRISB = 0b00000011; //0 = Output, 1 = Input PORTB = 0b00000000; // RC7 - Serial RX Pin 18 // RC6 - Serial TX Pin 17 // I2C: // RC3 - SCL = I2C clock Pin 14 // RC4 - SDA = I2C data Pin 15 TRISC = 0b10011000; //0 = Output, 1 = Input PORTC = 0b01000000; //RBPU_ = 0; //bitclr(OPTION,RBPU_); //Each of the PORTB pins has a weak internal pull-up. A //single control bit can turn on all the pull-ups. This is //performed by clearing bit RBPU (OPTION_REG<7>). //The weak pull-up is automatically turned off when the //port pin is configured as an output. The pull-ups are //disabled on a Power-on Reset. // RE3 (pin1) MCLR == input //TRISE = 0x00001000; PORTE = 0b11111111; INT0_ENBL = 0; // disable external interrupt for GYRO 1 INT1IE = 0; // disbalke external interrupt for GYRO2 #else // done _18F2321 #ifdef _16F884 ANSEL = 0b00000000; //Turn pins to Digital instead of Analog CM2CON0 = 0b00000111; //Turn off comparator on RA port CM1CON0 = 0b00000111; // for each unit it is individual // RA0,1,2,3,4 this will be stepper motor control 1A,2A,1B,2B,ENBL //TRISA = 0b10100000; //0 = Output, 1 = Input //PORTA = 0b00000000; // serial FLASH pin assignment // SSCLOCK RA7(pin16), SSDATA_IN RA6(pin15), SSDATA_OUT RA4, SSCS RA3 // 0 0 IN 1 TRISA = 0b00110000; //0 = Output, 1 = Input PORTA = 0b00001000; // SSCS set high // RB0 - external INT Pin 6 TRISB = 0b00000001; //0 = Output, 1 = Input PORTB = 0b11111110; // RC6 - Serial Out Pin 25 // RC7 - Serial In Pin 26 // I2C: // RC3 - SCL = I2C clock Pin 18 // RC4 - SDA = I2C data Pin 23 // RB0 - external INT Pin 33 TRISC = 0b10011000; //0 = Output, 1 = Input PORTC = 0b01000000; //RBPU_ = 0; //bitclr(OPTION,RBPU_); //Each of the PORTB pins has a weak internal pull-up. A //single control bit can turn on all the pull-ups. This is //performed by clearing bit RBPU (OPTION_REG<7>). //The weak pull-up is automatically turned off when the //port pin is configured as an output. The pull-ups are //disabled on a Power-on Reset. TRISD = 0b11111111; //0 = Output, 1 = Input PORTD = 0b00000000; // RE7 (pin1) MCLR == input TRISE = 0b11111111; //0 = Output, 1 = Input TRISE = 0b10000000; INT0_ENBL = 0; // disable external interrupt for GYRO 1 #else // done _18F2321 & _16F884 ANSEL = 0b00000000; //Turn pins to Digital instead of Analog CMCON = 0b00000111; //Turn off comparator on RA port // for each unit it is individual // RA0,1,2,3,4 this will be stepper motor control 1A,2A,1B,2B,ENBL //TRISA = 0b10100000; //0 = Output, 1 = Input //PORTA = 0b00000000; // RA5 MCLR == input // serial FLASH pin assignment // SSCLOCK RA7(pin16), SSDATA_IN RA6(pin15), SSDATA_OUT RA4(pin3), SSCS RA3(pin2) // 0 0 IN 1 TRISA = 0b00110000; //0 = Output, 1 = Input PORTA = 0b00001000; // SSCS set high // RB5 - Serial Out Pin 11 // RB2 - Serial In Pin 8 // I2C: // RB4 - SCL = I2C clock Pin 10 // RB1 - SDA = I2C data Pin 7 // RB0 - external INT Pin 6 // RB2 - serial input // RB5 - serial out TRISB = 0b00010111; //0 = Output, 1 = Input PORTB = 0b11111111; //RBPU_ = 0; //bitclr(OPTION,RBPU_); //Each of the PORTB pins has a weak internal pull-up. A //single control bit can turn on all the pull-ups. This is //performed by clearing bit RBPU (OPTION_REG<7>). //The weak pull-up is automatically turned off when the //port pin is configured as an output. The pull-ups are //disabled on a Power-on Reset. INT0_ENBL = 0; // disable external interrupt for GYRO 1 #endif #endif //RBIF = 0; //RBIE = 1; enable_uart(); //Setup the hardware UART for 20MHz at 9600bps // next two bits has to be set after all intialization done //PEIE = 1; // bit 6 PEIE: Peripheral Interrupt Enable bit // 1 = Enables all unmasked peripheral interrupts // 0 = Disables all peripheral interrupts //GIE = 1; // bit 7 GIE: Global Interrupt Enable bit // 1 = Enables all unmasked interrupts // 0 = Disables all interrupts enable_I2C(); TIMER0_INT_FLG = 0; // clean timer0 interrupt TIMER0_INT_ENBL = 0; // diasable timer0 interrupt TMR1IF = 0; // clean timer0 interrupt TMR1IE = 0; // diasable timer0 interrupt INT0_EDG = 0; // high -> low == interrupt INT0_FLG = 0; // clean extrnal interrupt RB0 pin 6 #ifdef _18F2321 INT1IF = 0; INTEDG1 = 0; #endif //INT0IE = 1; // enable external interrupt #endif } void ShowMessage(void) { // if message initiated by unit needs to check then it is possible to do: while(!Main.prepStream) // this will wait untill no relay message { } // in a case of a CMD replay it is safly to skip that check - unit allow to send message in CMD mode putch(UnitADR); // this message will circle over com and will be supressed by unit Puts("~"); putch(UnitADR); } #ifdef INCLUDE_CALIBR void InitCalibr() { unsigned char iCount; unsigned char FlashStatus; iSampleHH = 0x03; iSampleH = 0; for (iCount = 0; iCount < 32;iCount ++) // 256K { CS_LOW; SendSSByte(0x06); // "write enable" CS_HIGH; nop(); CS_LOW; SendSSByte(0x20); // "sector 4096 bytes erase" 0x030000, 0x031000, 0x032000. . . SendSSByte(iSampleHH); SendSSByte(iSampleH); SendSSByteFAST(0x00); CS_HIGH; iSampleH+=16; if (Carry) iSampleHH++; CS_LOW; SendSSByte(0x05); // "read status register" CHECK_BUSY: FlashStatus = GetSSByte(); if (bittest(FlashStatus,0)) // operation erase in a progress goto CHECK_BUSY; CS_HIGH; } iSampleH = 0; iSampleL = 0; iSampleHH = 3; DataB1.FirstSkip = 1; i8=0; } unsigned char ProcessCalibr() { unsigned char FlashStatus; register unsigned char W; #ifdef _DO_CONTINUE if (DataB1.SecondCalibr) // on a second just output { // wait for comm queue to be empty while(AOutQu.iQueueSize) { } } else { putch(UnitFrom); putch(SendCMD); } while(AInI2CQu.iQueueSize) { putch(getchI2C()); // process all responce == // it is OK - it process with GyroStatusIs or AllGyro.GyroDataIs // in first priority }; if (DataB1.SecondCalibr) { putch(UnitFrom); // close packet DataB1.SecondCalibr = 0; // and wait when it will ends while(AOutQu.iQueueSize) { } } else DataB1.SecondCalibr = 1; #else //CHECK_BUSY2: // CS_LOW; // SendSSByte(0x05); // "read status register" // FlashStatus = GetSSByte(); // if (bittest(FlashStatus,0)) // operation erase in a progress // goto CHECK_BUSY2; // CS_HIGH; // // nop();nop();nop(); //if (DataB1.FirstSkip) // Idle run for 4 seconds //{ // W = 1; // iSampleL+=W; // if (Carry) // { // W = 1; // iSampleH+=W; // if (Carry) // iSampleHH++; // if (iSampleHH == 0x01) // 4 seconds // { // DataB1.FirstSkip = 0; // iSampleH = 0; // iSampleL = 0; // iSampleHH = 0; // } // } // while(AInI2CQu.iQueueSize) // { // FlashStatus =getchI2C(); // } // return 0; //} if (i8 == 0) { CS_LOW; SendSSByte(0x06); // "write enable" CS_HIGH; nop(); CS_LOW; SendSSByteFAST(0x02); // "write/page programm" SendSSByte(iSampleHH); // address SendSSByte(iSampleH); SendSSByte(iSampleL); } while(AInI2CQu.iQueueSize) { SendSSByte(getchI2C()); // process all responce == i8++; } if (i8 == 64) // 8 samples done { //SendSSByte(AllGyro.CalibrUnit); CS_HIGH; iSampleL+=64; if (Carry) { W = 1; iSampleH+=W; if (Carry) iSampleHH++; } i8=0; } //if (iSampleH == 0x80) if (iSampleHH == 0x05) return 1; return 0; #endif } #endif // INCLUDE_CALIBR #include "commc8.h"