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@22:1cbdbc856660, 2013-05-19 (annotated)

Committer:
jekain314
Date:
Sun May 19 16:26:28 2013 +0000
Revision:
22:1cbdbc856660
Parent:
6:71da5b99de97
Child:
29:dead10cce6e9
added WMsg to all messages to PC to improve startup. Blink LEDs on 1PPS to indicate main loop activity

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 22:1cbdbc856660 25 const unsigned char IMUrecArraySize = 30;
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 6:71da5b99de97 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 6:71da5b99de97 120 //toPC.printf(" finished setting the default values\n");
jekain314 0:432b860b6ff7 121
jekain314 0:432b860b6ff7 122 }