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



File content as of revision 30:96d133f3008e:

//set up the SPI on pins 5, 6, 7 to read from the ADIS16488
SPI spi(p5, p6, p7); // mosi (DIN), miso (DOUT), sclk (CLK)
DigitalOut ADIS_CS(p8);     //Chip Select for the ADIS SPI
InterruptIn ADIS_DR(p28);   //DataReady interrupt connected to DIO2 for ADIS
DigitalOut ADIS_RST(p20);   //ADIS reset pin

bool IMUDataReady = false;
int IMURecordCounter = 0;
//see Table 9 from page 11 of the ADIS16488 spec 
//see fig 15 of spec -- note the low byte of the regsiter word is always zero
unsigned short LOW_REGISTER[]  = {0x4000, 0x4400, 0x4800, 0x4C00, 0x5000, 0x5400};   
unsigned short HIGH_REGISTER[] = {0x4200, 0x4600, 0x4A00, 0x4E00, 0x5200, 0x5600}; 

volatile unsigned long IMUtimeFrom1PPS = 0;
volatile int IMUClockCounter = 0;            //counter for IMU samples per sec

union WD { long dataWord; unsigned short pt[2];} wd;

//IMU records are buffered in the IMUDataReady ISR
const unsigned char IMUrecArraySize = 5;

#pragma pack(1)
struct IMUREC
    unsigned long GPSTime;
    long dataWord[6];

IMUREC imuPing[IMUrecArraySize];
IMUREC imuPong[IMUrecArraySize];
IMUREC tempRec;
volatile bool fillingPingWritingPong = true;

unsigned long maxDelIMUmsecs = 0;
unsigned long delIMUmsecs = 0;      
unsigned long lastIMUmsecs = 0; 

void IMUDataReadyISR(void)
    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;  //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
    {[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); 
        if (i<5)  // dont this on the last because this was pre-called
        {[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); }
        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;

void setupADIS(void)
    ADIS_RST = 0;
    //  set the IMU dataReady ISR
    // Setup the mbed SPI for 16 bit data, high steady state clock,
    // second edge capture, with a 1MHz clock rate
    ADIS_CS = 1;  //CS must be set high before it goes low cause the enable is the transition
    ADIS_RST = 1;
    ADIS_CS = 0; //set the Chip select low to enable the IMU SPI access
    spi.write((int)0x8003);  //change to page 3
    //change the DECRATE to 98.4 Hz (this is also in page 3)
    //the 8 sets the high bit to 1 indicating a write to a register
    // 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)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
    //the high byte is CD indicating the synch is enabled on the rising edge of the input clock  
    //spi.write((int)0x86CD);    //write high byte to register 0x06
    //spi.write((int)0x8700);    //write the low byte of 00 to registed 0x07
    //change the page to 0 to get the data
    spi.write((int)0x8000);  //change to page 0