this version has all of Jim's fixes for reading the GPS and IMU data synchronously
Dependencies: MODSERIAL SDFileSystem mbed SDShell CRC CommHandler FP LinkedList LogUtil
Diff: ADIS16488.h
- Revision:
- 30:96d133f3008e
- Parent:
- 29:dead10cce6e9
--- a/ADIS16488.h Thu Jan 09 14:09:05 2014 +0000 +++ b/ADIS16488.h Mon Mar 03 13:19:31 2014 +0000 @@ -22,7 +22,7 @@ union WD { long dataWord; unsigned short pt[2];} wd; //IMU records are buffered in the IMUDataReady ISR -const unsigned char IMUrecArraySize = 10; +const unsigned char IMUrecArraySize = 5; #pragma pack(1) struct IMUREC @@ -42,16 +42,27 @@ void IMUDataReadyISR(void) { - IMUtimeFrom1PPS = timeFromPPS.read_us(); + IMURecordCounter++; + + IMUtimeFrom1PPS = timeFromPPS.read_us(); //timer reset to zero in the the GPS 1PPS + + // GPSTimemsecs is taken from the GPS message -- but that may follow the 1PPS by several millisecs + // PPSTimeOffset is set to zero after the time becomes available + // so PPSTimeOffset accounts for the possible >1sec between the current IMU time and the time of the last PPS + // this also accounts for a missed GPS time message that holds the GPS time tempRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0; + //tempRec.GPSTime = PPSCounter*1000 + timeFromPPS.read_us()/1000; + + //test to see if we are ready to write the current IMU data buffer and swap the ping-pong buffer if (IMUClockCounter == IMUrecArraySize ) { - IMUDataReady = true; - fillingPingWritingPong = !fillingPingWritingPong; - IMUClockCounter = 0; + IMUDataReady = true; //signals the write in the main loop + fillingPingWritingPong = !fillingPingWritingPong; //swap the ping-pong buffer + IMUClockCounter = 0; //reset the IMU record counter } + // spi.write((int) HIGH_REGISTER[0]); //next read will return results from HIGH_REGITER[0] for (int i=0; i<6; i++) //read the 6 rate and accel variables { @@ -61,13 +72,16 @@ if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long - } + } + // //fill the correct buffer ping or pong if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec; else imuPong[IMUClockCounter] = tempRec; + // IMUClockCounter++; + return; } @@ -97,7 +111,8 @@ // The C abd D designate the registers for the DECRATE of Page 3 // The 0x17 sets the rate to: 2460/(23+1) = 102.5Hz // The 0x18 sets the rate to: 2460/(24+1) = 98.4Hz - spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte) + //spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte) + spi.write((int)0x8C30); //write high byte (only page number can be written in a single byte) spi.write((int)0x8D00); //write the low byte of DECRATE //to set the GPS VARF clock as the input synch clock for the IMU