#include "CRC.c" #include #use rs232(baud=9600,parity=N,xmit=PIN_B1,rcv=PIN_B0,bits=8,stream=COMPSTREAM,ERRORS,DISABLE_INTS) /** * This struct holds the data parsed from the compass * **/ struct { float heading; //heading of compass float temp; //temperature float pitch; //pitch float roll; //roll } COMPASS; //------OPTIONS---- #define COMP_TIMEOUT 12000 //Time to wait for incoming rs232 characters #define IGNORE_CRC 0 //IGNORE CRC ERRORS #define OUTPUT_ERR 1 //OUTPUT ERRORS OVER SERIAL //-----FLAGS------- int1 comp_awake = 0; //is compass awake int1 dataset_sent = 0; //have we specified what data we want, only needs to be done once per startup int1 comp_timeout_error = 0; //raised when timeout waiting for character int1 comp_bad_crc = 0; //raised when CRC check doesn't match - this doesn't always mean bad data //-----Bad Data Thresholds------ //these values are the reasonable boundaries for which BAD CRC data will be accepted #define HEADING_THRESH 5 #define TEMP_THRESH 2 #define PITCH_THRESH 3 #define ROLL_THRESH 3 //----COMM STRINGS--- int const awaken[5] = {0x00,0x05,0x17,0x9D,0x23}; //wakes up compass int const dataset[10] = {0x00,0x0A,0x03,0x04,0x05,0x07,0x18,0x19,0x84,0xBF}; //sets data components to recieve, currently heading, temp, pitch, roll int const polldata[5] = {0x00,0x05,0x04,0xBF,0x71}; //requests the data //-----BUFFER------ int compstr[48] = ""; //holds incoming compass data /** * This function will wait for a incoming character a certain amount of time * until it times out and raises the timeout_error flag **/ char comp_timed_getc() { int32 timeout=0; comp_timeout_error=FALSE; while(!kbhit(COMPSTREAM)&&(timeout 360)) { //data unaceptable return; } COMPASS.heading = f; break; case 0x07: //TEMPERATURE f = f_IEEEtoPIC(value); if(comp_bad_crc && !(abs(COMPASS.temp-f)<=TEMP_THRESH)) { //data unaceptable return; } COMPASS.temp = f; break; case 0x18: //PITCH ANGLE f = f_IEEEtoPIC(value); if(comp_bad_crc && !(abs(COMPASS.pitch-f)<=PITCH_THRESH)) { //data unaceptable return; } COMPASS.pitch = f; break; case 0x19: //ROLL ANGLE f = f_IEEEtoPIC(value); if(comp_bad_crc && !(abs(COMPASS.roll-f)<=ROLL_THRESH)) { //data unaceptable return; } COMPASS.roll = f; break; } } /** * This function updates the global COMPASS variable * by polling the compass for data **/ void update_compass() { //Variable definitions int i=0; //used for outside looping int length=0; //length of bytes recieved int expect=0; //expected number of bytes to be recieved int16 lCRC=0; //the local calculated CRC16 int16 rCRC=0; //the remote CRC //lower flags in case they're raised comp_bad_crc = 0; comp_timeout_error = 0; //check if compass is awake, if(!comp_awake) { //awaken compass for(i=0;i<5;i++) { fputc(awaken[i],COMPSTREAM); } //set flag comp_awake = 1; } //check if dataset has been sent if(!dataset_sent) { //send it for(i=0;i<10;i++) { fputc(dataset[i],COMPSTREAM); } //set flag dataset_sent = 1; } //clear existing buffer for(i=0;i<50;i++) { compstr[i] = 0x00; } //poll compass for data for(i=0;i<5;i++) { fputc(polldata[i],COMPSTREAM); } //------recieve data----- i=0; //reset counter comp_timeout_error = 0; //reset timeout flag while(!comp_timeout_error) { compstr[i] = comp_timed_getc(); //fills compstr with data //only accept the number of bytes specified by the second bit if(i==1) { expect = compstr[i]; //byte count } i++; //increment i, which now equals number of bytes recieved //if number of bytes recieved equals expected, break loop if(expect == i && i != 0) { //we've recieved all bytes we're expecting //increment i to make length definition below match i++; break; } } length=i-1; //set length equal to the number of bytes recieved //-----verify data------- //make sure we recieved bytes if(length == 0) { if(OUTPUT_ERR) { fprintf(BASESTREAM,"$ERR14:NO COMP BYTES\r\n"); return; } } //make sure byte count matches //compstr[1] = byte count if(compstr[1] != length) { //there was an error in reciept, report it if(OUTPUT_ERR) fprintf(BASESTREAM,"$ERR15:BYTE CHECK FAILED,%X,%X\r\n",compstr[1],length); return; } //check if CRC16 Matches lCRC = generate_16bit_crc(compstr, length-2, CRC_CCITT); //local crc rCRC = make16(compstr[length-2],compstr[length-1]); //remote crc if(lCRC != rCRC && !IGNORE_CRC) { if(OUTPUT_ERR) fprintf(BASESTREAM,"$ERR16:CRC CHECK FAILED,%LX,%LX\r\n",lCRC,rCRC); comp_bad_crc = TRUE; //return; } //-----Parse Data---- //the packet is bytes 2 to length-2 //the frame is byte 3, it should be 0x05 but we don't need it //the number of parameters is byte 4, but we don't use it //the parameter components are five bytes long, the first byte is the DataCompoentID, the next four are the value for(i=4;i<(length-2);i+=5) { parse_comp(compstr[i],make32(compstr[i+1],compstr[i+2],compstr[i+3],compstr[i+4])); } //-----Adjust for true magnetic declination----- //E-subtract from heading //W-add to heading if(GPS.magVarDir == 'E' && GPS.status =='A') { //subtract COMPASS.heading = (COMPASS.heading - GPS.magVar); } else if(GPS.magVarDir == 'W' && GPS.status =='A') { //add COMPASS.heading = (COMPASS.heading + GPS.magVar); } //-----DEBUG OUTPUT---- /* //fprintf(BASESTREAM,"\r\nTIMED OUT AFTER:%d Characters\r\n",i); fprintf(BASESTREAM,"$HDG:%f$TEMP:%f$ROLL:%f$PITCH:%f\r\n",COMPASS.heading,COMPASS.temp,COMPASS.roll,COMPASS.pitch); //put out all recieved for(i=0;i<30;i++) { putc(compstr[i]); } */ }