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?
User | Revision | Line number | New 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 |