james kain / Mbed 2 deprecated Waldo_Embed_V2

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

ADIS16488.h@30:96d133f3008e, 2014-03-03 (annotated)

Committer:
jekain314
Date:
Mon Mar 03 13:19:31 2014 +0000
Revision:
30:96d133f3008e
Parent:
29:dead10cce6e9 
commit of RT_Download.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jekain314 0:432b860b6ff7 1
jekain314 0:432b860b6ff7 2
jekain314 0:432b860b6ff7 3
jekain314 0:432b860b6ff7 4 //set up the SPI on pins 5, 6, 7 to read from the ADIS16488
jekain314 0:432b860b6ff7 5 SPI spi(p5, p6, p7); // mosi (DIN), miso (DOUT), sclk (CLK)
jekain314 0:432b860b6ff7 6 DigitalOut ADIS_CS(p8); //Chip Select for the ADIS SPI
jekain314 0:432b860b6ff7 7 InterruptIn ADIS_DR(p28); //DataReady interrupt connected to DIO2 for ADIS
jekain314 0:432b860b6ff7 8 DigitalOut ADIS_RST(p20); //ADIS reset pin
jekain314 0:432b860b6ff7 9
jekain314 0:432b860b6ff7 10 bool IMUDataReady = false;
jekain314 0:432b860b6ff7 11 int IMURecordCounter = 0;
jekain314 0:432b860b6ff7 12 //see Table 9 from page 11 of the ADIS16488 spec
jekain314 0:432b860b6ff7 13 //see fig 15 of spec -- note the low byte of the regsiter word is always zero
jekain314 0:432b860b6ff7 14 // X_DELTANG_LOW, Y_DELTANG_LOW, X_DETANG_LOW, X_DELTVEL_LOW, Y_DELTVEL_LOW, Z_DELTVEL_LOW
jekain314 0:432b860b6ff7 15 unsigned short LOW_REGISTER[] = {0x4000, 0x4400, 0x4800, 0x4C00, 0x5000, 0x5400};
jekain314 0:432b860b6ff7 16 // X_DELTANG_HIGH, Y_DELTANG_HIGH, X_DETANG_HIGH, X_DELTVEL_HIGH, Y_DELTVEL_HIGH, Z_DELTVEL_HIGH
jekain314 0:432b860b6ff7 17 unsigned short HIGH_REGISTER[] = {0x4200, 0x4600, 0x4A00, 0x4E00, 0x5200, 0x5600};
jekain314 0:432b860b6ff7 18
jekain314 0:432b860b6ff7 19 volatile unsigned long IMUtimeFrom1PPS = 0;
jekain314 0:432b860b6ff7 20 volatile int IMUClockCounter = 0; //counter for IMU samples per sec
jekain314 0:432b860b6ff7 21
jekain314 0:432b860b6ff7 22 union WD { long dataWord; unsigned short pt[2];} wd;
jekain314 0:432b860b6ff7 23
jekain314 0:432b860b6ff7 24 //IMU records are buffered in the IMUDataReady ISR
jekain314 30:96d133f3008e 25 const unsigned char IMUrecArraySize = 5;
jekain314 0:432b860b6ff7 26
jekain314 0:432b860b6ff7 27 #pragma pack(1)
jekain314 0:432b860b6ff7 28 struct IMUREC
jekain314 0:432b860b6ff7 29 {
jekain314 0:432b860b6ff7 30 unsigned long GPSTime;
jekain314 0:432b860b6ff7 31 long dataWord[6];
jekain314 0:432b860b6ff7 32 };
jekain314 0:432b860b6ff7 33
jekain314 0:432b860b6ff7 34 IMUREC imuPing[IMUrecArraySize];
jekain314 0:432b860b6ff7 35 IMUREC imuPong[IMUrecArraySize];
jekain314 0:432b860b6ff7 36 IMUREC tempRec;
jekain314 0:432b860b6ff7 37 volatile bool fillingPingWritingPong = true;
jekain314 0:432b860b6ff7 38
jekain314 0:432b860b6ff7 39 unsigned long maxDelIMUmsecs = 0;
jekain314 0:432b860b6ff7 40 unsigned long delIMUmsecs = 0;
jekain314 0:432b860b6ff7 41 unsigned long lastIMUmsecs = 0;
jekain314 0:432b860b6ff7 42
jekain314 0:432b860b6ff7 43 void IMUDataReadyISR(void)
jekain314 0:432b860b6ff7 44 {
jekain314 30:96d133f3008e 45 IMURecordCounter++;
jekain314 30:96d133f3008e 46
jekain314 30:96d133f3008e 47 IMUtimeFrom1PPS = timeFromPPS.read_us(); //timer reset to zero in the the GPS 1PPS
jekain314 30:96d133f3008e 48
jekain314 30:96d133f3008e 49 // GPSTimemsecs is taken from the GPS message -- but that may follow the 1PPS by several millisecs
jekain314 30:96d133f3008e 50 // PPSTimeOffset is set to zero after the time becomes available
jekain314 30:96d133f3008e 51 // so PPSTimeOffset accounts for the possible >1sec between the current IMU time and the time of the last PPS
jekain314 30:96d133f3008e 52 // this also accounts for a missed GPS time message that holds the GPS time
jekain314 0:432b860b6ff7 53 tempRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0;
jekain314 0:432b860b6ff7 54
jekain314 30:96d133f3008e 55 //tempRec.GPSTime = PPSCounter*1000 + timeFromPPS.read_us()/1000;
jekain314 30:96d133f3008e 56
jekain314 30:96d133f3008e 57 //test to see if we are ready to write the current IMU data buffer and swap the ping-pong buffer
jekain314 0:432b860b6ff7 58 if (IMUClockCounter == IMUrecArraySize )
jekain314 0:432b860b6ff7 59 {
jekain314 30:96d133f3008e 60 IMUDataReady = true; //signals the write in the main loop
jekain314 30:96d133f3008e 61 fillingPingWritingPong = !fillingPingWritingPong; //swap the ping-pong buffer
jekain314 30:96d133f3008e 62 IMUClockCounter = 0; //reset the IMU record counter
jekain314 0:432b860b6ff7 63 }
jekain314 0:432b860b6ff7 64
jekain314 30:96d133f3008e 65 //
jekain314 0:432b860b6ff7 66 spi.write((int) HIGH_REGISTER[0]); //next read will return results from HIGH_REGITER[0]
jekain314 0:432b860b6ff7 67 for (int i=0; i<6; i++) //read the 6 rate and accel variables
jekain314 0:432b860b6ff7 68 {
jekain314 0:432b860b6ff7 69 wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]);
jekain314 0:432b860b6ff7 70 if (i<5) // dont this on the last because this was pre-called
jekain314 0:432b860b6ff7 71 { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); }
jekain314 0:432b860b6ff7 72
jekain314 0:432b860b6ff7 73 if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long
jekain314 0:432b860b6ff7 74 else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long
jekain314 30:96d133f3008e 75 }
jekain314 30:96d133f3008e 76 //
jekain314 29:dead10cce6e9 77
jekain314 29:dead10cce6e9 78 //fill the correct buffer ping or pong
jekain314 0:432b860b6ff7 79 if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec;
jekain314 0:432b860b6ff7 80 else imuPong[IMUClockCounter] = tempRec;
jekain314 30:96d133f3008e 81 //
jekain314 0:432b860b6ff7 82
jekain314 0:432b860b6ff7 83 IMUClockCounter++;
jekain314 30:96d133f3008e 84
jekain314 0:432b860b6ff7 85
jekain314 0:432b860b6ff7 86 return;
jekain314 0:432b860b6ff7 87 }
jekain314 0:432b860b6ff7 88
jekain314 0:432b860b6ff7 89 void setupADIS(void)
jekain314 0:432b860b6ff7 90 {
jekain314 0:432b860b6ff7 91 ADIS_DR.mode(PullDown);
jekain314 0:432b860b6ff7 92 ADIS_RST = 0;
jekain314 0:432b860b6ff7 93
jekain314 0:432b860b6ff7 94 // set the IMU dataReady ISR
jekain314 0:432b860b6ff7 95 ADIS_DR.rise(&IMUDataReadyISR);
jekain314 0:432b860b6ff7 96
jekain314 0:432b860b6ff7 97 // Setup the mbed SPI for 16 bit data, high steady state clock,
jekain314 0:432b860b6ff7 98 // second edge capture, with a 1MHz clock rate
jekain314 0:432b860b6ff7 99 spi.format(16,3);
jekain314 0:432b860b6ff7 100 spi.frequency(5000000);
jekain314 0:432b860b6ff7 101
jekain314 0:432b860b6ff7 102 ADIS_CS = 1; //CS must be set high before it goes low cause the enable is the transition
jekain314 0:432b860b6ff7 103 ADIS_RST = 1;
jekain314 0:432b860b6ff7 104 wait(0.5);
jekain314 0:432b860b6ff7 105 ADIS_CS = 0; //set the Chip select low to enable the IMU SPI access
jekain314 0:432b860b6ff7 106
jekain314 0:432b860b6ff7 107 spi.write((int)0x8003); //change to page 3
jekain314 0:432b860b6ff7 108
jekain314 0:432b860b6ff7 109 //change the DECRATE to 98.4 Hz (this is also in page 3)
jekain314 0:432b860b6ff7 110 //the 8 sets the high bit to 1 indicating a write to a register
jekain314 0:432b860b6ff7 111 // The C abd D designate the registers for the DECRATE of Page 3
jekain314 0:432b860b6ff7 112 // The 0x17 sets the rate to: 2460/(23+1) = 102.5Hz
jekain314 0:432b860b6ff7 113 // The 0x18 sets the rate to: 2460/(24+1) = 98.4Hz
jekain314 30:96d133f3008e 114 //spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte)
jekain314 30:96d133f3008e 115 spi.write((int)0x8C30); //write high byte (only page number can be written in a single byte)
jekain314 0:432b860b6ff7 116 spi.write((int)0x8D00); //write the low byte of DECRATE
jekain314 0:432b860b6ff7 117
jekain314 0:432b860b6ff7 118 //to set the GPS VARF clock as the input synch clock for the IMU
jekain314 0:432b860b6ff7 119 //the high byte is CD indicating the synch is enabled on the rising edge of the input clock
jekain314 0:432b860b6ff7 120 //spi.write((int)0x86CD); //write high byte to register 0x06
jekain314 0:432b860b6ff7 121 //spi.write((int)0x8700); //write the low byte of 00 to registed 0x07
jekain314 0:432b860b6ff7 122
jekain314 0:432b860b6ff7 123 //change the page to 0 to get the data
jekain314 0:432b860b6ff7 124 spi.write((int)0x8000); //change to page 0
jekain314 0:432b860b6ff7 125
jekain314 0:432b860b6ff7 126 }
jekain314 29:dead10cce6e9 127
jekain314 29:dead10cce6e9 128