#class auto #memmap xmem //expand memory alloted to program #use "gps.lib" //used to convert GPS sentence to coordinates //Define GPS serial buffer size #define CINBUFSIZE 127 #define COUTBUFSIZE 127 //Define camera serial buffer sizes #define BINBUFSIZE 511 #define BOUTBUFSIZE 127 //Define SD serial buffer sizes #define DINBUFSIZE 127 #define DOUTBUFSIZE 1023 //Define OBD serial buffer sizes #define EINBUFSIZE 127 #define EOUTBUFSIZE 127 #define YawAverage 2090 //yaw rate average offset used to calibrate yaw rate gyro #define DEBUGSYS 0 //if 1 use printf statements //if 0 use MS delays #define PrintSDData 0 //Print each SD data line before saving to SD #define PrintSDCall 0 //Print indicators of when SD is being accessed #define EnableSleepy 1 //Allow system to enter sleepy mode when engine off // Cap the amount of attempts each device will be tested #define OBDTESTLIMIT 5 #define OBDdelay 10 #define GPSTESTLIMIT 120 #define TIMTESTLIMIT 120 #define ACLTESTLIMIT 50 #define YAWTESTLIMIT 30 #define CAMTESTLIMIT 220 #define camBaudRate 57600 #define camDelay 250 #define SD_FLAG_DELAY 15 void bootSystem(); //boot sequence function int systemGTG; //is entire system functioning? //YAW RATE GYRO routines & variables int prepYaw(); void getYawRate(); char yawSign; int yawRateG; //global location for yaw data int yawGTG; //is yaw rate gyro good to go? int yawRate; //value returned from yawRate grab int resetShadow; //used during yaw polling //ACCELEROMETER routines & variables void AccelInit(); int prepAccel(); void getAccel(); struct { float x; // acceleration in units of (g) float y; // acceleration in units of (g) float x1; // filtered x accel char signx1; float y1; // filtered y accel char signy1; }Accelleration; char capture_status; unsigned long pulse_widthX; unsigned long pulse_widthY; int accJarCounter; //used for sleepy mode int accelGTG; //is accel good to go? //OBD routines & variables int prepOBD(); float getRPM(); cofunc int getRPMMT(); cofunc int getSpeedMT(); cofunc int getThrotMT(); float hex2dec(char[]); //convert elm327 response to dec int globalRPM; //vehicle rpm int globalSpeed; //vehicle speed int globalThrot; //vehicle throttle position int OBDGTG; //is OBD goot to go? //GPS routines & variables int prepGPS(); void getGPS(); cofunc int getGPSMT(); int GPSchar; char GPSdata[100]; int GPSdataPointer; GPSPosition current_pos; struct tm current_time; int degreesLat; int degreesLon; float minutesLat; float minutesLon; char directionLat; char directionLon; int timeStamp[6]; int GPSGTG; //is GPS good to go? //GPS data is global so not initalized here int GPSpollCounterA; //used to block excessive GPS calls int GPSpollCounterB; //used to block excessive GPS calls //used to generate timer if GPS lock not obtained int timeOffset; int myTime; //CAMERA routines & variables int prepCam(); cofunc int getPic(); //Camera picture arrays char picture[4095]; char picture2[4095]; char picture3[4095]; char picPacket[508]; int camGTG; //is camera good to go? //arrays used to communicate with the camera char camSync[6]; char camSyncR[6]; char camReceive[6]; char camInit[6]; char camPkgSize[6]; char camSnapShot[6]; char camGetPic[6]; char camAck[6]; char camReset[6]; char camSleep[6]; char endPic[6]; //camera responses char tempString[15]; char tempString2[25]; //SD MODULE routines & variables static const long baud_rate = 460800L; int prepSD(); void resetSD(); int initSD(); void writeSDcBc(char[]); cofunc int writeSD3(char[]); cofunc int writeSDcam(int); cofunc int writeSDcamTime(); cofunc int SDwrites(char[], int, int); int SDGTG; //is Camera good to go? int usingSD; //flag to block multiple sd calls at once char currentFileName[19]; //string used to open current data file char currentFileNamePic[19]; //string used to open current picture file char SDdata[101]; //data to be writen to sd //Non multitasking delays void giveTime(); void giveMTime(); void giveSTime(); void giveXSTime(); //LED controls void lightsOff(); void lightsOn(); void flashLED1(); void flashLED2(); void flashLED3(); void flashLED4(); void flashLED5(); void flashLED6(); void flashLED7(); void testLEDs(); void signalMaster(); void main() { int x; bootSystem(); //boos sequence call while (1) { /* poll gps if fails use rabbit timer for time */ costate { waitfor(serCrdUsed()>70); if(GPSpollCounterA==0 || GPSpollCounterB==0) { wfd getGPSMT(); if(directionLon!='N' && directionLon!='W' && directionLon!='S' && directionLon!='E') { myTime = (SEC_TIMER+timeOffset); timeStamp[0]=0; timeStamp[1]=0; timeStamp[2]=0; timeStamp[3]=myTime/3600; timeStamp[4]=(myTime-(timeStamp[3]*3600))/60; timeStamp[5]=myTime%60; } } else waitfor(DelayMs(150)); } //poll yaw rate gyro costate { if(yawGTG) { waitfor(DelayMs(100)); getYawRate(); } } //poll accelerometer costate { if(accelGTG) { getAccel(); } } //poll OBD costate { if(OBDGTG) { wfd getRPMMT(); wfd getThrotMT(); wfd getSpeedMT(); } } //poll camera costate { if(camGTG) { wfd getPic(); } } //store current data to SD module costate { //turn data into set length string sprintf(SDdata,"%02d-%02d-%04d %02d:%02d:%02d $$ %02d %09f %c %03d %09f %c $$ %c%02d $$ %5.2f %5.2f $$ %04d %03d %03d\r\n\0", timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3], timeStamp[4], timeStamp[5], degreesLat, minutesLat, directionLat, degreesLon, minutesLon, directionLon, yawSign, yawRateG, Accelleration.x1, Accelleration.y1, globalRPM, globalSpeed, globalThrot); GPSpollCounterA=0; //reset GPS location poll counter, allowing it to get data again GPSpollCounterB=0; //reset GPS time poll counter, allowing it to get data again waitfor(usingSD==0); //Waitfor SD module to be free usingSD=1; //Lock down SD module preventing any other device to write to it wfd writeSD3(SDdata); //write current batch of data to SD usingSD=0; //release SD, allows other devices to use for writing waitfor(DelayMs(60)); //give time for devices to poll new data } //enter sleepy mode if criteria met costate { waitfor(DelaySec(15)); if(EnableSleepy && OBDGTG && accelGTG && globalRPM<100) { //Wait for SD to be free then lock down waitfor(usingSD==0); usingSD=1; waitfor(DelaySec(3)); lightsOff(); //Shut off voltage regulator BitWrPortI(PADR, &PADRShadow, 0, 0); giveTime(); //Poll accel to steady output before monitoring for(x=0;x<350;++x) getAccel(); while(1) { getAccel(); giveXSTime(); //Check if any movement detected if(Accelleration.x1 >= 0.25 && Accelleration.x1 <= 0.7 || Accelleration.x1 <= -0.25 && Accelleration.x1 >= -0.7) { ++accJarCounter; //If jaring sustained exit hibernation if(accJarCounter>25) { giveTime(); bootSystem(); break; } } } } } } } void bootSystem() { giveMTime(); if(DEBUGSYS)printf("Boot System Function Entered\n"); if(DEBUGSYS)printf("Checking Devices...\n"); //Reset all global variables and flags yawGTG=0; yawRate=0; accelGTG=0; GPSGTG=0; OBDGTG=0; camGTG=0; SDGTG=0; systemGTG=0; globalRPM=0; globalSpeed=0; globalThrot=0; GPSpollCounterA=0; GPSpollCounterB=0; accJarCounter=0; usingSD=0; brdInit(); //required to use rabbits communication ports lightsOff(); //reset LEDs if(DEBUGSYS)printf("\n Turning on 5V regulator."); WrPortI(SPCR, NULL, 0x084); // Set Parallel Port A as Output to control the 5V Voltage reg BitWrPortI(PADR, &PADRShadow, 1, 0); // 5V Regulator ON if(DEBUGSYS)printf("\n 5V regulator on."); if(DEBUGSYS) { printf("\nTESTING LEDS\n"); giveTime(); flashLED1(); flashLED2(); flashLED3(); flashLED4(); flashLED5(); flashLED6(); flashLED7(); giveTime(); } //Notify User of Power Up signalMaster(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing OBD Test\n"); } //OBDGTG = prepOBD(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing YAW Test\n"); } yawGTG = prepYaw(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing ACL Test\n"); } accelGTG = prepAccel(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing GPS Test\n"); } GPSGTG = prepGPS(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing CAM Test\n"); } camGTG = prepCam(); signalMaster(); if(DEBUGSYS) { giveMTime(); printf("Performing SD Test\n"); } SDGTG = prepSD(); signalMaster(); if(DEBUGSYS)printf("Signaling ALL Test Complete on ALL LED\n"); flashLED1(); flashLED2(); flashLED3(); flashLED4(); flashLED5(); flashLED6(); flashLED7(); lightsOff(); //Report boot errors if(accelGTG==0) WrPortI(PEB3R, NULL, 0xFF); else { Accelleration.x1=0.0; Accelleration.y1=0.0; } if(OBDGTG==0) WrPortI(PEB4R, NULL, 0xFF); if(yawGTG==0) WrPortI(PEB5R, NULL, 0xFF); if(camGTG==0) WrPortI(PEB6R, NULL, 0xFF); if(GPSGTG==0) { if(SEC_TIMER<0) timeOffset = SEC_TIMER - (2*SEC_TIMER); else timeOffset = -1*SEC_TIMER; WrPortI(PEB7R, NULL, 0xFF); } if(SDGTG==0) { while(1) { giveSTime(); WrPortI(PEB1R, NULL, 0xFF); WrPortI(PEB0R, NULL, 0xFF); giveMTime(); WrPortI(PEB1R, NULL, 0x00); WrPortI(PEB0R, NULL, 0x00); giveSTime(); } } if(SDGTG && GPSGTG && camGTG && yawGTG && accelGTG && OBDGTG) systemGTG=1; if(systemGTG==0) WrPortI(PEB1R, NULL, 0xFF); if(DEBUGSYS)printf("\n\nBoot Report-\nYaw Rate Gyro:%d\nAccelerometer:%d\nOBD:%d\nGPS:%d\nCamera:%d\nSD:%d\nSystem:%d\n\n",yawGTG,accelGTG,OBDGTG,GPSGTG,camGTG,SDGTG,systemGTG); } /* BEGIN: YAW RATE GYRO FUNCTIONS */ int prepYaw(){ int testValue; int testCounter; int passed; testCounter=0; passed=0; testValue = 3000; while(testCounter=-5 && yawRateG<=5) { passed=1; break; } else ++testCounter; } if(passed) return 1; else { yawSign = '+'; yawRateG = 0; return 0; } } void getYawRate() { int i; int DOUT[17]; int decimalSum; DOUT[0]=0; //Turn on Yaw LED WrPortI(PEB5R, NULL, 0xFF); BitWrPortI(PFDR, PFDRShadow, 0, 7); //CS low BitWrPortI(PFDR, PFDRShadow, 1, 2); //WRITE for(i=0;i<15;i++); BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[1]=BitRdPortI(PFDR, 5); //DOUT low BitWrPortI(PFDR, PFDRShadow, 0, 2); //LOW BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[2]=BitRdPortI(PFDR, 5); //DOUT low BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[3]=BitRdPortI(PFDR, 5); //DOUT ADD1 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[4]=BitRdPortI(PFDR, 5); //DOUT ADD0 BitWrPortI(PFDR, PFDRShadow, 0, 2); //ADD! BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[5]=BitRdPortI(PFDR, 5); //DOUT DB11 BitWrPortI(PFDR, PFDRShadow, 0, 2); //ADD0 BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[6]=BitRdPortI(PFDR, 5); //DOUT DB10 BitWrPortI(PFDR, PFDRShadow, 1, 2); //HIGH BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[7]=BitRdPortI(PFDR, 5); //DOUT DB9 BitWrPortI(PFDR, PFDRShadow, 1, 2); //HIGH BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[8]=BitRdPortI(PFDR, 5); //DOUT DB8 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[9]=BitRdPortI(PFDR, 5); //DOUT DB7 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[10]=BitRdPortI(PFDR, 5); //DOUT DB6 BitWrPortI(PFDR, PFDRShadow, 0, 2); //LOW BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[11]=BitRdPortI(PFDR, 5); //DOUT DB5 BitWrPortI(PFDR, PFDRShadow, 1, 2); //CODING BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[12]=BitRdPortI(PFDR, 5); //DOUT DB4 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[13]=BitRdPortI(PFDR, 5); //DOUT DB3 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[14]=BitRdPortI(PFDR, 5); //DOUT DB2 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high DOUT[15]=BitRdPortI(PFDR, 5); //DOUT DB1 BitWrPortI(PFDR, PFDRShadow, 0, 2); //DC BitWrPortI(PFDR, PFDRShadow, 0, 0); //SCLK low BitWrPortI(PFDR, PFDRShadow, 1, 0); //SCLK high BitWrPortI(PFDR, PFDRShadow, 1, 7); //CS high decimalSum = DOUT[15]; decimalSum += DOUT[14]*2; decimalSum += DOUT[13]*4; decimalSum += DOUT[12]*8; decimalSum += DOUT[11]*16; decimalSum += DOUT[10]*32; decimalSum += DOUT[9]*64; decimalSum += DOUT[8]*128; decimalSum += DOUT[7]*256; decimalSum += DOUT[6]*512; decimalSum += DOUT[5]*1024; decimalSum += DOUT[4]*2048; //Turn off Yaw LED WrPortI(PEB5R, NULL, 0x00); //calibrate yaw rate decimalSum = decimalSum - YawAverage; decimalSum /= (YawAverage/300); //store yaw rate to global variable if(decimalSum >= 0) { yawSign = '+'; if(decimalSum<=99) yawRateG = decimalSum; else yawRateG = 99; } else { yawSign = '-'; if(decimalSum>=-99) yawRateG = decimalSum - (2*decimalSum); else yawRateG = 99; } } /* END: YAW RATE GYRO FUNCTIONS */ /* BEGIN: ACCELEROMETER FUNCTIONS */ void AccelInit() { WrPortI(ICS1R, NULL, 0x88); //PF1 is pulse capture pin for channel 1 - X WrPortI(ICS2R, NULL, 0x99); //PF3 is pulse capture pin for channel 2 - Y WrPortI(TAT8R, NULL, 50 - 1); //(TA8 prescaler) WrPortI(ICCSR, NULL, 0x0c); //zero out counters WrPortI(ICCR, NULL, 0x00); //no interrupts //run counter start to stop //start is falling edge, stop is rising edge //latch counter on stop WrPortI(ICT1R, NULL, 0x59); WrPortI(ICT2R, NULL, 0x59); pulse_widthX = 0; pulse_widthY = 0; getAccel(); giveTime(); getAccel(); } int prepAccel() { int testCounter; int passed; testCounter=0; passed=0; AccelInit(); while(testCounter=-10 && Accelleration.x<=10 && Accelleration.y>=-10 && Accelleration.y<=10) { passed=1; break; } } if(passed) return 1; else { //zero out accelerometer if failed Accelleration.signx1 = '+'; Accelleration.x1 = 0; Accelleration.signy1 = '+'; Accelleration.y1 = 0; return 0; } } void getAccel() { int AccelDelta; AccelDelta=0; WrPortI(PEB3R, NULL, 0xFF); //LED Control //listen for capture states capture_status = RdPortI(ICCSR); if(capture_status & 0x10) { //channel 1 stop occured //read capture value, LSB first pulse_widthX += RdPortI(ICL1R); pulse_widthX += RdPortI(ICM1R)*256; Accelleration.x = (((pulse_widthX/29420.0)-0.5)/0.3); pulse_widthX = 0; WrPortI(ICCSR, NULL, 0x04); //zero out counter } else if(capture_status & 0x04) { if(DEBUGSYS)printf("Input capture counter rolled over.\n"); pulse_widthX += 0x10000; } if(capture_status & 0x40) { //channel 2 stop occured //read capture value, LSB first pulse_widthY += RdPortI(ICL2R); pulse_widthY += RdPortI(ICM2R)*256; Accelleration.y = (((pulse_widthY/29420.0)-0.5)/0.3); pulse_widthY = 0; WrPortI(ICCSR, NULL, 0x08); //zero out counter } else if(capture_status & 0x08) { if(DEBUGSYS)printf("Input capture counter rolled over.\n"); pulse_widthY += 0x10000; } WrPortI(PEB3R, NULL, 0x00); //LED Control if((Accelleration.x+0.21) >= -1.2 && (Accelleration.x+0.21) <= 1.2) { Accelleration.x1=Accelleration.x+0.21; } if((Accelleration.y+0.183) >= -1.2 && (Accelleration.y+0.183) <= 1.2) { Accelleration.y1=Accelleration.y+0.183; } } /* END: ACCELEROMETER FUNCTIONS */ /* BEGIN: OBD FUNCTIONS */ int prepOBD() { int fetchedRPM; int fetchedSpeed; int fetchedThrot; int testCounter; int passed; fetchedRPM=0; fetchedSpeed=0; fetchedThrot=0; testCounter=0; passed=0; while(testCounter5) { passed=1; break; } else { //setup ports and request RPM serEclose(); giveSTime(); WrPortI(PGDDR, &PGDDRShadow, 0x40); serEopen(38400); giveMTime(); serErdFlush(); serEwrFlush(); serEputs("AT Z\r"); giveTime(); giveTime(); fetchedRPM=getRPM(); } } if(passed) return 1; else { globalRPM=0; globalSpeed=0; globalThrot=0; return 0; } } float getRPM() { char readSentence[40]; char OBDresponse[5]; float carRPM; int x; int loopExit; int loopPass; int RTSvar; int RTRvar; int p; //reset flags and variables carRPM=0; loopExit=1; loopPass=1; RTSvar=0; RTRvar=0; WrPortI(PEB4R, NULL, 0xFF); //LED CONTROL //Clear Buffer serErdFlush(); serEwrFlush(); p=0; while(BitRdPortI(PGDR,4)==1 && p<15) { if(DEBUGSYS)printf("waiting OBD...\n"); giveSTime(); ++p; } serEputs("01 0C\r"); //request rpm //WrPortI(PEB4R, NULL, 0x00); //LED CONTROL giveMTime(); //WrPortI(PEB4R, NULL, 0xFF); //LED CONTROL giveMTime(); WrPortI(PEB4R, NULL, 0x00); //LED CONTROL p=0; while(BitRdPortI(PGDR,4)==1 && p<15) { if(DEBUGSYS)printf("waiting OBD...\n"); giveSTime(); ++p; } WrPortI(PEB4R, NULL, 0xFF); //LED CONTROL serEread(readSentence, serErdUsed(), 100); if(DEBUGSYS) { printf("\n"); for(p=0;p<40;++p) printf(" %c",readSentence[p]); printf("\n"); } //decode elm327 response for(x=0;x<35;x++) { if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ') { if(DEBUGSYS)printf("OBD ACK Received\n"); OBDresponse[0]=readSentence[x+6]; OBDresponse[1]=readSentence[x+7]; OBDresponse[2]=readSentence[x+9]; OBDresponse[3]=readSentence[x+10]; OBDresponse[4]='\0'; if(DEBUGSYS)printf("%c%c%c%c\n",OBDresponse[0],OBDresponse[1],OBDresponse[2],OBDresponse[3]); carRPM = hex2dec(OBDresponse); carRPM = carRPM/4.0; globalRPM = carRPM; } } WrPortI(PEB4R, NULL, 0x00); //LED CONTROL return carRPM; } cofunc int getRPMMT() { char readSentence[40]; char OBDresponse[5]; float carRPM; int x; int loopExit; int loopPass; int RTSvar; int RTRvar; int p; //reset variables and flags carRPM=0; loopExit=1; loopPass=1; RTSvar=0; RTRvar=0; WrPortI(PEB4R, NULL, 0xFF); //LED CONTROL //reset buffers serErdFlush(); serEwrFlush(); waitfor(0==BitRdPortI(PGDR, 4)); serEputs("01 0C\r"); //request rpms waitfor(DelayMs(OBDdelay)); waitfor(0==BitRdPortI(PGDR, 4)); serEread(readSentence, serErdUsed(), 100); WrPortI(PEB4R, NULL, 0x00); //LED CONTROL //decode elm327 response for(x=0;x<35;x++) { if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ') { printf("OBD ACK Received\n"); OBDresponse[0]=readSentence[x+6]; OBDresponse[1]=readSentence[x+7]; OBDresponse[2]=readSentence[x+9]; OBDresponse[3]=readSentence[x+10]; OBDresponse[4]='\0'; carRPM = hex2dec(OBDresponse); carRPM = carRPM/4.0; globalRPM = carRPM; } } return 1; } cofunc int getSpeedMT() { char readSentence[25]; char OBDresponse[5]; float carSpeed; int x; carSpeed = 0; WrPortI(PEB4R, NULL, 0xFF); //LED CONTROLS //reset buffers serErdFlush(); serEwrFlush(); waitfor(0==BitRdPortI(PGDR, 4)); serEputs("01 0D\r"); //request speed waitfor(DelayMs(OBDdelay)); waitfor(0==BitRdPortI(PGDR, 4)); serEread(readSentence, 20, 10); WrPortI(PEB4R, NULL, 0x00); //LED CONTROLS //decode elm327 response for(x=0;x<14;x++) { if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ') { OBDresponse[0]='0'; OBDresponse[1]='0'; OBDresponse[2]=readSentence[x+6]; OBDresponse[3]=readSentence[x+7]; OBDresponse[4]='\0'; carSpeed = hex2dec(OBDresponse); carSpeed = carSpeed/1.609; globalSpeed = carSpeed; } } return 1; } cofunc int getThrotMT() { char readSentence[25]; char OBDresponse[5]; int carThrot; int x; carThrot=0; //WrPortI(PEB4R, NULL, 0xFF); //LED CONTROLS //flush buffers serErdFlush(); serEwrFlush(); waitfor(0==BitRdPortI(PGDR, 4)); serEputs("01 11\r"); //request throotle position waitfor(DelayMs(OBDdelay)); waitfor(0==BitRdPortI(PGDR, 4)); serEread(readSentence, 20, 10); //decode elm327 response for(x=0;x<14;x++) { if(readSentence[x]=='4' && readSentence[x+1]=='1' && readSentence[x+2]==' ') { OBDresponse[0]='0'; OBDresponse[1]='0'; OBDresponse[2]=readSentence[x+6]; OBDresponse[3]=readSentence[x+7]; OBDresponse[4]='\0'; carThrot = hex2dec(OBDresponse); carThrot = carThrot*100; carThrot = carThrot/255; globalThrot = carThrot; } } return 1; } /* conver elm327 return data to decimal for processing */ float hex2dec(char hex[]) { float decValue; int x; int multip; int errorFlag; errorFlag=0; decValue = 0; x=0; multip=0; while(x<4){ if(x==0) multip=4096; else if(x==1) multip=256; else if(x==2) multip=16; else if(x==3) multip=1; else errorFlag=1; if(hex[x] >= 48 && hex[x] <= 57){ decValue = decValue + ((hex[x]-48)*multip); } else if(hex[x] == 'a' || hex[x] == 'A') { decValue = decValue + (10*multip); } else if(hex[x] == 'b' || hex[x] == 'B'){ decValue = decValue + (11*multip); } else if(hex[x] == 'c' || hex[x] == 'C') { decValue = decValue + (12*multip); } else if(hex[x] == 'd' || hex[x] == 'D') { decValue = decValue + (13*multip); } else if(hex[x] == 'e' || hex[x] == 'E') { decValue = decValue + (14*multip); } else if(hex[x] == 'f' || hex[x] == 'F'){ decValue = decValue + (15*multip); } else errorFlag=1; x++; } if(errorFlag) decValue = 0; return decValue; } /* END: OBD FUNCTIONS */ int prepGPS() { int testCount; int passed; testCount=0; passed=0; serCopen(4800); giveTime(); while(testCount0) { WrPortI(PEB7R, NULL, 0x00); //led control GPSdata[GPSdataPointer] = GPSchar; GPSdataPointer++; if(GPSdataPointer == 100) GPSdataPointer = 0; } } WrPortI(PEB7R, NULL, 0x00); //led control } /* fetches GPS sentence from GPS recievier and converts * * to decimal position and time information MULTITASKING */ cofunc int getGPSMT() { int loopBreaker; GPSdataPointer=0; loopBreaker = 1; WrPortI(PEB7R, NULL, 0xFF); while(loopBreaker) { wfd GPSchar = cof_serCgetc(); if(GPSchar == '\r' || GPSchar == '\n') { loopBreaker=0; GPSdata[GPSdataPointer]=0; GPSdataPointer=0; if(gps_get_position(¤t_pos, GPSdata) == 0) { degreesLat = current_pos.lat_degrees; degreesLon = current_pos.lon_degrees; minutesLat = current_pos.lat_minutes; minutesLon = current_pos.lon_minutes; directionLat = current_pos.lat_direction; directionLon = current_pos.lon_direction; GPSpollCounterA=1; } if(gps_get_utc(¤t_time, GPSdata) == 0) { timeStamp[0]=current_time.tm_mon; timeStamp[1]=current_time.tm_mday; timeStamp[2]=current_time.tm_year+1900; timeStamp[3]=current_time.tm_hour; timeStamp[4]=current_time.tm_min; timeStamp[5]=current_time.tm_sec; GPSpollCounterB=1; } } else if(GPSchar>0) { GPSdata[GPSdataPointer] = GPSchar; GPSdataPointer++; if(GPSdataPointer == 100) GPSdataPointer = 0; } } WrPortI(PEB7R, NULL, 0x00); if(timeStamp[0] <= 0 || timeStamp[0] > 12) { timeStamp[0]=0; timeStamp[1]=0; timeStamp[2]=0; timeStamp[3]=0; timeStamp[4]=0; timeStamp[5]=0; } return 1; } int prepCam() { //looping variables int i; int j; int k; int m; int jMul; int camResetSync; //amount of sync requests int packetPrinted; //lines of picture data fetched int bufferUsed; //data in buffer int bufferGrabed; //data grabbed from buffer //variables used for camera data processing and reporting char hexChar[2]; int passed; int byteTracker[35]; int packetSize[35]; int totalPic; int packFetchCalls; int lastPacket; int camI; int camPacketSize; int picUpperLimit; int picUpperLimit2; int picUpperLimit3; int packetReq; //reset flags & variables and declare comunication arrays packetReq=0; camPacketSize=512; i=0; j=0; camResetSync = 100; totalPic = 0; lastPacket = 0; bufferGrabed = 0; jMul=0; bufferUsed = 0; passed=0; hexChar[0] = "0"; hexChar[1] = "1"; camSync[0] = 0xAA; camSync[1] = 0x0D; camSync[2] = 0x00; camSync[3] = 0x00; camSync[4] = 0x00; camSync[5] = 0x00; camSyncR[0] = 0xAA; camSyncR[1] = 0x0E; camSyncR[2] = 0x0D; camSyncR[3] = 0x00; camSyncR[4] = 0x00; camSyncR[5] = 0x00; camReceive[0] = 0x00; camReceive[1] = 0x00; camReceive[2] = 0x00; camReceive[3] = 0x00; camReceive[4] = 0x00; camReceive[5] = 0x00; camInit[0] = 0xAA; camInit[1] = 0x01; camInit[2] = 0x00; camInit[3] = 0x07; camInit[4] = 0x03; camInit[5] = 0x03; camPkgSize[0] = 0xAA; camPkgSize[1] = 0x06; camPkgSize[2] = 0x08; camPkgSize[3] = 0x00; camPkgSize[4] = 0x02; camPkgSize[5] = 0x00; camSnapShot[0] = 0xAA; camSnapShot[1] = 0x05; camSnapShot[2] = 0x00; camSnapShot[3] = 0x00; camSnapShot[4] = 0x00; camSnapShot[5] = 0x00; camGetPic[0] = 0xAA; camGetPic[1] = 0x04; camGetPic[2] = 0x01; camGetPic[3] = 0x00; camGetPic[4] = 0x00; camGetPic[5] = 0x00; camAck[0] = 0xAA; camAck[1] = 0x0E; camAck[2] = 0x00; camAck[3] = 0x00; camAck[4] = 0x00; camAck[5] = 0x00; camReset[0] = 0xAA; camReset[1] = 0x08; camReset[2] = 0x00; camReset[3] = 0x00; camReset[4] = 0x00; camReset[5] = 0xFF; camSleep[0] = 0xAA; camSleep[1] = 0x09; camSleep[2] = 0x00; camSleep[3] = 0x00; camSleep[4] = 0x00; camSleep[5] = 0x00; endPic[0] = 0xAA; endPic[1] = 0x0E; endPic[2] = 0x00; endPic[3] = 0x00; endPic[4] = 0xF0; endPic[5] = 0xF0; //open camera connection serBclose(); serBopen(camBaudRate); serBwrFlush(); serBrdFlush(); while(i4); for(t=0;t<4;++t) camIDSize[t]=serBgetc(); if(camIDSize[2]==0xFA && camIDSize[3]==0x01) { //Waitfor picture packet & fetch waitfor(serBrdUsed()==508); wfd cof_serBread(picPacket,508,300); //Lock down SD for picture writing waitfor(usingSD==0); usingSD=1; wfd writeSDcam(506); usingSD=0; } else { //READ LAST PACKET SIZE IN HEX CONVERT TO DEC lastPacketSizeHEX[0]=camIDSize[3]; lastPacketSizeHEX[1]=camIDSize[2]; if(lastPacketSizeHEX[0]>0) kHex2DecHelper=lastPacketSizeHEX[0] + 255; else kHex2DecHelper=0; //find last packet picture size k2Hex2DecHelper=lastPacketSizeHEX[1]; kHex2DecHelper=kHex2DecHelper+k2Hex2DecHelper; waitfor(serBrdUsed()>=kHex2DecHelper+2); //Plus 2 for check sum wfd cof_serBread(picPacket,kHex2DecHelper+2,300); waitfor(usingSD==0); usingSD=1; wfd writeSDcam(kHex2DecHelper); //Check sum no longer needed usingSD=0; gettingPic=0; //Picture Caught Break } camAck[4]=camAck[4] + 0x01; } WrPortI(PEB6R, NULL, 0x00); //LED Control serBwrite(endPic,6); waitfor(DelayMs(camDelay)); i=300; passed=1; } } } } } } } //giveSTime(); waitfor(DelayMs(camDelay)); ++i; WrPortI(PEB6R, NULL, 0x00); if(i%70==0) { //if camera not communicating send reset call if(DEBUGSYS)printf("\n\n\n\nAttempting Camera Reset\n"); serBclose(); serBopen(camBaudRate); serBwrFlush(); serBrdFlush(); if(DEBUGSYS)printf("Sending Camera Reset\n"); serBwrite(camReset, 6); waitfor(DelayMs(camDelay)); } } if(passed) { return 1; } else return 0; } int prepSD() { int i; int result; i=0; result=0; while(i<15 && result==0) { if(DEBUGSYS)printf("SD Test: %d\n",i); WrPortI(PEB1R, NULL, 0xFF); //LED Control WrPortI(PEB0R, NULL, 0xFF); //LED Control result = initSD(); WrPortI(PEB1R, NULL, 0x00); //LED Control WrPortI(PEB0R, NULL, 0x00); //LED Control giveSTime(); } if(result) return 1; else return 0; } int initSD() { char fileName[37]; char SDresponse[100]; int passed; int defaultNamer; int timeGrabbed; //looping variables int i; int j; int k; //reset variables timeGrabbed=0; k=0; passed=0; //open SD port and set up communication resetSD(); serDopen(9600); giveSTime(); writeSDcBc("B 05A9\r"); giveSTime(); serDclose(); giveSTime(); serDopen(baud_rate); giveSTime(); serDrdFlush(); writeSDcBc("# 1\r"); writeSDcBc("I\r"); if(GPSGTG) { //grab time from gps while(k 12)) { if(DEBUGSYS)printf("pulling time: %d, abort @ 150\n", k); giveSTime(); getGPS(); ++k; } if(kDL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]); //make currentFileName Write if(DEBUGSYS)printf("\nName for file: %s\n",currentFileName); writeSDcBc(currentFileName); j=sprintf(currentFileName, "O 1A>DL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]); //make currentFileName Append if(camGTG) { //if camera operational use time from GPS to make data file name j=sprintf(currentFileNamePic, "O 2W>PL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]); //make currentFileName Write if(DEBUGSYS)printf("\nName for pic file: %s\n",currentFileNamePic); writeSDcBc(currentFileNamePic); j=sprintf(currentFileNamePic, "O 2A>PL%02d%02d%02d.TXT\r",timeStamp[1], timeStamp[3], timeStamp[4]); } } } if(timeGrabbed==0) { //if unable to grab time from GPS make default name //make default name from system timer defaultNamer = SEC_TIMER; if(DEBUGSYS)printf("\n%d\n",defaultNamer); if(defaultNamer < 0) defaultNamer = -1*defaultNamer; while(defaultNamer>9999) { defaultNamer = defaultNamer/10; } if(DEBUGSYS)printf("\n%04d\n",defaultNamer); j=sprintf(currentFileName, "O 1W>DLOG%04d.TXT\r", defaultNamer); if(DEBUGSYS)printf("\n%s\n",currentFileName); writeSDcBc(currentFileName); j=sprintf(currentFileName, "O 1A>DLOG%04d.TXT\r", defaultNamer); if(camGTG) { j=sprintf(currentFileNamePic, "O 2W>PLOG%04d.TXT\r", defaultNamer); //make currentFileNamePic Write if(DEBUGSYS)printf("\nName for pic file: %s\n",currentFileNamePic); writeSDcBc(currentFileNamePic); j=sprintf(currentFileNamePic, "O 2A>PLOG%04d.TXT\r", defaultNamer); } } //send file headers writeSDcBc("W 1>D0\r"); writeSDcBc("UTC Time $$ UTC Position(Lat,Lon) $$ Yaw $$ Accel(x,y) $$ OBD(RPM,Speed,Throttle)\r\n"); writeSDcBc("------------------------------------------------------------------------------------------------------\r\n"); writeSDcBc("F 1\r"); //ensure write operation was sucesful serDread(SDresponse,serDrdUsed(),400); if(DEBUGSYS) { printf("%c\n",SDresponse[28]); printf("%c\n",SDresponse[29]); printf("%c\n",SDresponse[30]); } if(SDresponse[28] == '!' && SDresponse[29] == '0' && SDresponse[30] == '0') return 1; else return 0; } void resetSD() { BitWrPortI(PBDR, &resetShadow, 0, 6); giveSTime(); BitWrPortI(PBDR, &resetShadow, 1, 6); giveSTime(); } //upload one character at a time to SD module void writeSDcBc(char str2write[]) { int j; int loopBreaker; loopBreaker = 0; for(j=0; j10); wfd cof_serDputc(toBeWritten[j]); } return 1; } //send latest sensor data to data file cofunc int writeSD3(char data[]) { WrPortI(PEB0R, NULL, 0xFF); //LED SIGNAL waitfor(serDwrUsed()==0); wfd cof_serDputs(currentFileName); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs("W 1>5D\r"); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs(data); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs("F 1\r"); WrPortI(PEB0R, NULL, 0x00); //LED SIGNAL } cofunc int writeSDcam(int picPacketSize) { char sizeDecleration[15]; int totalpicPacketSize; int l; waitfor(serDwrUsed()==0); wfd cof_serDputs(currentFileNamePic); //Find total Size of Data to be written totalpicPacketSize = ((picPacketSize*2)); //Create Size Decleration sprintf(sizeDecleration, "%s%X%s", "W 2>", totalpicPacketSize, "\r"); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs(sizeDecleration); //Send data to SD one HEX byte at a time waitfor(serDwrFree()>=1014); for(l=0;l", strlen(tempString2),"\r"); wfd cof_serDputs(sizeDecleration); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs(tempString2); while(BitRdPortI(PBDR, 5)) { printf("wait "); waitfor(DelayMs(SD_FLAG_DELAY)); } wfd cof_serDputs("F 2\r"); } /* BEGIN: OTHER FUNCTIONS */ //Time Delay Functions void giveTime() { int giveTimeX; int giveTimeY; for(giveTimeX=0;giveTimeX<=1000;giveTimeX++) for(giveTimeY=0;giveTimeY<=1000;giveTimeY++) giveTimeY = giveTimeY * 1; } void giveMTime() { int giveTimeX; int giveTimeY; for(giveTimeX=0;giveTimeX<=500;giveTimeX++) for(giveTimeY=0;giveTimeY<=500;giveTimeY++) giveTimeY = giveTimeY * 1; } void giveSTime() { int giveTimeX; int giveTimeY; for(giveTimeX=0;giveTimeX<=250;giveTimeX++) for(giveTimeY=0;giveTimeY<=250;giveTimeY++) giveTimeY = giveTimeY * 1; } void giveXSTime() { int giveTimeX; int giveTimeY; for(giveTimeX=0;giveTimeX<=25;giveTimeX++) for(giveTimeY=0;giveTimeY<=25;giveTimeY++) giveTimeY = giveTimeY * 1; } //LED Functions void lightsOff() { WrPortI(PEB0R, NULL, 0x00); WrPortI(PEB1R, NULL, 0x00); WrPortI(PEB3R, NULL, 0x00); WrPortI(PEB4R, NULL, 0x00); WrPortI(PEB5R, NULL, 0x00); WrPortI(PEB6R, NULL, 0x00); WrPortI(PEB7R, NULL, 0x00); } void lightsOn() { WrPortI(PEB0R, NULL, 0xFF); WrPortI(PEB1R, NULL, 0xFF); WrPortI(PEB3R, NULL, 0xFF); WrPortI(PEB4R, NULL, 0xFF); WrPortI(PEB5R, NULL, 0xFF); WrPortI(PEB6R, NULL, 0xFF); WrPortI(PEB7R, NULL, 0xFF); } void flashLED1() { WrPortI(PEB3R, NULL, 0xFF); giveSTime(); WrPortI(PEB3R, NULL, 0x00); } void flashLED2() { WrPortI(PEB4R, NULL, 0xFF); giveSTime(); WrPortI(PEB4R, NULL, 0x00); } void flashLED3() { WrPortI(PEB5R, NULL, 0xFF); giveSTime(); WrPortI(PEB5R, NULL, 0x00); } void flashLED4() { WrPortI(PEB6R, NULL, 0xFF); giveSTime(); WrPortI(PEB6R, NULL, 0x00); } void flashLED5() { WrPortI(PEB7R, NULL, 0xFF); giveSTime(); WrPortI(PEB7R, NULL, 0x00); } void flashLED6() { WrPortI(PEB0R, NULL, 0xFF); giveSTime(); WrPortI(PEB0R, NULL, 0x00); } void flashLED7() { WrPortI(PEB1R, NULL, 0xFF); giveSTime(); WrPortI(PEB1R, NULL, 0x00); } void testLEDs() { lightsOff(); giveMTime(); lightsOn(); giveMTime(); lightsOff(); giveMTime(); lightsOn(); giveMTime(); lightsOff(); giveMTime(); lightsOn(); giveMTime(); lightsOff(); giveMTime(); lightsOn(); giveMTime(); lightsOff(); } void signalMaster() { WrPortI(PEB1R, NULL, 0xFF); giveSTime(); WrPortI(PEB1R, NULL, 0x00); giveSTime(); WrPortI(PEB1R, NULL, 0xFF); giveSTime(); WrPortI(PEB1R, NULL, 0x00); giveSTime(); } /* END: OTHER FUNCTIONS */