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@0:432b860b6ff7, 2013-04-22 (annotated)

Committer:
jekain314
Date:
Mon Apr 22 21:26:04 2013 +0000
Revision:
0:432b860b6ff7
Child:
6:71da5b99de97
does not work yet with mbed_test. Correctly collects GPS and IMU data.

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 0:432b860b6ff7 25 const unsigned char IMUrecArraySize = 15;
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 synch;
jekain314 0:432b860b6ff7 31 unsigned short msgID;
jekain314 0:432b860b6ff7 32 unsigned long GPSTime;
jekain314 0:432b860b6ff7 33 long dataWord[6];
jekain314 0:432b860b6ff7 34 // 4 + 2 + 4 + 24 = 34
jekain314 0:432b860b6ff7 35 };
jekain314 0:432b860b6ff7 36
jekain314 0:432b860b6ff7 37 IMUREC imuPing[IMUrecArraySize];
jekain314 0:432b860b6ff7 38 IMUREC imuPong[IMUrecArraySize];
jekain314 0:432b860b6ff7 39 IMUREC tempRec;
jekain314 0:432b860b6ff7 40 volatile bool fillingPingWritingPong = true;
jekain314 0:432b860b6ff7 41
jekain314 0:432b860b6ff7 42
jekain314 0:432b860b6ff7 43 unsigned long maxDelIMUmsecs = 0;
jekain314 0:432b860b6ff7 44 unsigned long delIMUmsecs = 0;
jekain314 0:432b860b6ff7 45 unsigned long lastIMUmsecs = 0;
jekain314 0:432b860b6ff7 46
jekain314 0:432b860b6ff7 47 void IMUDataReadyISR(void)
jekain314 0:432b860b6ff7 48 {
jekain314 0:432b860b6ff7 49 IMUtimeFrom1PPS = timeFromPPS.read_us();
jekain314 0:432b860b6ff7 50 tempRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0;
jekain314 0:432b860b6ff7 51
jekain314 0:432b860b6ff7 52 if (IMUClockCounter == IMUrecArraySize )
jekain314 0:432b860b6ff7 53 {
jekain314 0:432b860b6ff7 54 IMUDataReady = true;
jekain314 0:432b860b6ff7 55 fillingPingWritingPong = !fillingPingWritingPong;
jekain314 0:432b860b6ff7 56 IMUClockCounter = 0;
jekain314 0:432b860b6ff7 57 }
jekain314 0:432b860b6ff7 58
jekain314 0:432b860b6ff7 59 spi.write((int) HIGH_REGISTER[0]); //next read will return results from HIGH_REGITER[0]
jekain314 0:432b860b6ff7 60 for (int i=0; i<6; i++) //read the 6 rate and accel variables
jekain314 0:432b860b6ff7 61 {
jekain314 0:432b860b6ff7 62 wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]);
jekain314 0:432b860b6ff7 63 if (i<5) // dont this on the last because this was pre-called
jekain314 0:432b860b6ff7 64 { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); }
jekain314 0:432b860b6ff7 65
jekain314 0:432b860b6ff7 66 if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long
jekain314 0:432b860b6ff7 67 else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long
jekain314 0:432b860b6ff7 68 }
jekain314 0:432b860b6ff7 69
jekain314 0:432b860b6ff7 70 if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec;
jekain314 0:432b860b6ff7 71 else imuPong[IMUClockCounter] = tempRec;
jekain314 0:432b860b6ff7 72
jekain314 0:432b860b6ff7 73 IMUClockCounter++;
jekain314 0:432b860b6ff7 74
jekain314 0:432b860b6ff7 75 return;
jekain314 0:432b860b6ff7 76 }
jekain314 0:432b860b6ff7 77
jekain314 0:432b860b6ff7 78 void setupADIS(void)
jekain314 0:432b860b6ff7 79 {
jekain314 0:432b860b6ff7 80 ADIS_DR.mode(PullDown);
jekain314 0:432b860b6ff7 81 ADIS_RST = 0;
jekain314 0:432b860b6ff7 82
jekain314 0:432b860b6ff7 83 // set the IMU dataReady ISR
jekain314 0:432b860b6ff7 84 ADIS_DR.rise(&IMUDataReadyISR);
jekain314 0:432b860b6ff7 85
jekain314 0:432b860b6ff7 86 // Setup the mbed SPI for 16 bit data, high steady state clock,
jekain314 0:432b860b6ff7 87 // second edge capture, with a 1MHz clock rate
jekain314 0:432b860b6ff7 88 spi.format(16,3);
jekain314 0:432b860b6ff7 89 spi.frequency(5000000);
jekain314 0:432b860b6ff7 90
jekain314 0:432b860b6ff7 91 ADIS_CS = 1; //CS must be set high before it goes low cause the enable is the transition
jekain314 0:432b860b6ff7 92 ADIS_RST = 1;
jekain314 0:432b860b6ff7 93 wait(0.5);
jekain314 0:432b860b6ff7 94 ADIS_CS = 0; //set the Chip select low to enable the IMU SPI access
jekain314 0:432b860b6ff7 95
jekain314 0:432b860b6ff7 96 spi.write((int)0x8003); //change to page 3
jekain314 0:432b860b6ff7 97
jekain314 0:432b860b6ff7 98 //change the DECRATE to 98.4 Hz (this is also in page 3)
jekain314 0:432b860b6ff7 99 //the 8 sets the high bit to 1 indicating a write to a register
jekain314 0:432b860b6ff7 100 // The C abd D designate the registers for the DECRATE of Page 3
jekain314 0:432b860b6ff7 101 // The 0x17 sets the rate to: 2460/(23+1) = 102.5Hz
jekain314 0:432b860b6ff7 102 // The 0x18 sets the rate to: 2460/(24+1) = 98.4Hz
jekain314 0:432b860b6ff7 103 spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte)
jekain314 0:432b860b6ff7 104 spi.write((int)0x8D00); //write the low byte of DECRATE
jekain314 0:432b860b6ff7 105
jekain314 0:432b860b6ff7 106 //to set the GPS VARF clock as the input synch clock for the IMU
jekain314 0:432b860b6ff7 107 //the high byte is CD indicating the synch is enabled on the rising edge of the input clock
jekain314 0:432b860b6ff7 108 //spi.write((int)0x86CD); //write high byte to register 0x06
jekain314 0:432b860b6ff7 109 //spi.write((int)0x8700); //write the low byte of 00 to registed 0x07
jekain314 0:432b860b6ff7 110
jekain314 0:432b860b6ff7 111 //change the page to 0 to get the data
jekain314 0:432b860b6ff7 112 spi.write((int)0x8000); //change to page 0
jekain314 0:432b860b6ff7 113
jekain314 0:432b860b6ff7 114 toPC.printf(" setting the default values\n");
jekain314 0:432b860b6ff7 115
jekain314 0:432b860b6ff7 116 //set the IMU synch and message ID
jekain314 0:432b860b6ff7 117 tempRec.synch = 0x1C1244AA; //same as the GPS synch words
jekain314 0:432b860b6ff7 118 tempRec.msgID = 111; //IMU record ID
jekain314 0:432b860b6ff7 119
jekain314 0:432b860b6ff7 120 toPC.printf(" finished setting the default values\n");
jekain314 0:432b860b6ff7 121
jekain314 0:432b860b6ff7 122 }