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@29:dead10cce6e9, 2014-01-09 (annotated)
- Committer:
- jekain314
- Date:
- Thu Jan 09 14:09:05 2014 +0000
- Revision:
- 29:dead10cce6e9
- Parent:
- 22:1cbdbc856660
- Child:
- 30:96d133f3008e
initial commit
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 | 29:dead10cce6e9 | 25 | const unsigned char IMUrecArraySize = 10; |
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 | 0:432b860b6ff7 | 45 | IMUtimeFrom1PPS = timeFromPPS.read_us(); |
jekain314 | 0:432b860b6ff7 | 46 | tempRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0; |
jekain314 | 0:432b860b6ff7 | 47 | |
jekain314 | 0:432b860b6ff7 | 48 | if (IMUClockCounter == IMUrecArraySize ) |
jekain314 | 0:432b860b6ff7 | 49 | { |
jekain314 | 0:432b860b6ff7 | 50 | IMUDataReady = true; |
jekain314 | 0:432b860b6ff7 | 51 | fillingPingWritingPong = !fillingPingWritingPong; |
jekain314 | 0:432b860b6ff7 | 52 | IMUClockCounter = 0; |
jekain314 | 0:432b860b6ff7 | 53 | } |
jekain314 | 0:432b860b6ff7 | 54 | |
jekain314 | 0:432b860b6ff7 | 55 | spi.write((int) HIGH_REGISTER[0]); //next read will return results from HIGH_REGITER[0] |
jekain314 | 0:432b860b6ff7 | 56 | for (int i=0; i<6; i++) //read the 6 rate and accel variables |
jekain314 | 0:432b860b6ff7 | 57 | { |
jekain314 | 0:432b860b6ff7 | 58 | wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); |
jekain314 | 0:432b860b6ff7 | 59 | if (i<5) // dont this on the last because this was pre-called |
jekain314 | 0:432b860b6ff7 | 60 | { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); } |
jekain314 | 0:432b860b6ff7 | 61 | |
jekain314 | 0:432b860b6ff7 | 62 | if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long |
jekain314 | 0:432b860b6ff7 | 63 | else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long |
jekain314 | 0:432b860b6ff7 | 64 | } |
jekain314 | 29:dead10cce6e9 | 65 | |
jekain314 | 29:dead10cce6e9 | 66 | //fill the correct buffer ping or pong |
jekain314 | 0:432b860b6ff7 | 67 | if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec; |
jekain314 | 0:432b860b6ff7 | 68 | else imuPong[IMUClockCounter] = tempRec; |
jekain314 | 0:432b860b6ff7 | 69 | |
jekain314 | 0:432b860b6ff7 | 70 | IMUClockCounter++; |
jekain314 | 0:432b860b6ff7 | 71 | |
jekain314 | 0:432b860b6ff7 | 72 | return; |
jekain314 | 0:432b860b6ff7 | 73 | } |
jekain314 | 0:432b860b6ff7 | 74 | |
jekain314 | 0:432b860b6ff7 | 75 | void setupADIS(void) |
jekain314 | 0:432b860b6ff7 | 76 | { |
jekain314 | 0:432b860b6ff7 | 77 | ADIS_DR.mode(PullDown); |
jekain314 | 0:432b860b6ff7 | 78 | ADIS_RST = 0; |
jekain314 | 0:432b860b6ff7 | 79 | |
jekain314 | 0:432b860b6ff7 | 80 | // set the IMU dataReady ISR |
jekain314 | 0:432b860b6ff7 | 81 | ADIS_DR.rise(&IMUDataReadyISR); |
jekain314 | 0:432b860b6ff7 | 82 | |
jekain314 | 0:432b860b6ff7 | 83 | // Setup the mbed SPI for 16 bit data, high steady state clock, |
jekain314 | 0:432b860b6ff7 | 84 | // second edge capture, with a 1MHz clock rate |
jekain314 | 0:432b860b6ff7 | 85 | spi.format(16,3); |
jekain314 | 0:432b860b6ff7 | 86 | spi.frequency(5000000); |
jekain314 | 0:432b860b6ff7 | 87 | |
jekain314 | 0:432b860b6ff7 | 88 | ADIS_CS = 1; //CS must be set high before it goes low cause the enable is the transition |
jekain314 | 0:432b860b6ff7 | 89 | ADIS_RST = 1; |
jekain314 | 0:432b860b6ff7 | 90 | wait(0.5); |
jekain314 | 0:432b860b6ff7 | 91 | ADIS_CS = 0; //set the Chip select low to enable the IMU SPI access |
jekain314 | 0:432b860b6ff7 | 92 | |
jekain314 | 0:432b860b6ff7 | 93 | spi.write((int)0x8003); //change to page 3 |
jekain314 | 0:432b860b6ff7 | 94 | |
jekain314 | 0:432b860b6ff7 | 95 | //change the DECRATE to 98.4 Hz (this is also in page 3) |
jekain314 | 0:432b860b6ff7 | 96 | //the 8 sets the high bit to 1 indicating a write to a register |
jekain314 | 0:432b860b6ff7 | 97 | // The C abd D designate the registers for the DECRATE of Page 3 |
jekain314 | 0:432b860b6ff7 | 98 | // The 0x17 sets the rate to: 2460/(23+1) = 102.5Hz |
jekain314 | 0:432b860b6ff7 | 99 | // The 0x18 sets the rate to: 2460/(24+1) = 98.4Hz |
jekain314 | 0:432b860b6ff7 | 100 | spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte) |
jekain314 | 0:432b860b6ff7 | 101 | spi.write((int)0x8D00); //write the low byte of DECRATE |
jekain314 | 0:432b860b6ff7 | 102 | |
jekain314 | 0:432b860b6ff7 | 103 | //to set the GPS VARF clock as the input synch clock for the IMU |
jekain314 | 0:432b860b6ff7 | 104 | //the high byte is CD indicating the synch is enabled on the rising edge of the input clock |
jekain314 | 0:432b860b6ff7 | 105 | //spi.write((int)0x86CD); //write high byte to register 0x06 |
jekain314 | 0:432b860b6ff7 | 106 | //spi.write((int)0x8700); //write the low byte of 00 to registed 0x07 |
jekain314 | 0:432b860b6ff7 | 107 | |
jekain314 | 0:432b860b6ff7 | 108 | //change the page to 0 to get the data |
jekain314 | 0:432b860b6ff7 | 109 | spi.write((int)0x8000); //change to page 0 |
jekain314 | 0:432b860b6ff7 | 110 | |
jekain314 | 0:432b860b6ff7 | 111 | } |
jekain314 | 29:dead10cce6e9 | 112 | |
jekain314 | 29:dead10cce6e9 | 113 |