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@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?

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 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