imu-spi.cpp

00001 //
00002 // imu-spi.cpp
00003 //
00004 // copyright 2010 Hugh Shane
00005 //
00006 #include "mbed.h"
00007 #include "imu-spi.h"
00008 
00009 ImuSpi::ImuSpi(void) :
00010     spi(p11, p12, p13), // mosi, miso, sclk (Note: mbed uses SSPx for the SPI busses)
00011     cs(p16),            // IMU chip select, active low
00012     reset(p15),         // IMU reset, active low. Also resets the altimeter.
00013     diag(p17),          // diagnostic output
00014     imuDataReady(p14),  // IMU data-ready interrupt input, active falling edge
00015     onePPS(p19)         // GPS 1 PPS interrupt input, active rising edge
00016 {
00017     cs = 1; // set to quiescent state
00018     diag = 0;
00019     imuDataReady.mode(PullUp); // just to be safe
00020     sequenceNumber = 0;
00021     imuReadIndex = 0;
00022 
00023     // Initialize the IMU command sequence. Note: register 0x00 is a dummy 
00024     // value that triggers the last read in a burst cycle.
00025     int8_t regVals[] = {0x04,0x06,0x08,0x0A,0x0C,0x0E,0x3C,0x00};
00026     for (int i = 0; i < bufferSize; i++) imuRegs[i] = regVals[i];
00027     
00028     // init the pingpong buffer
00029     pingpong = 0;
00030     
00031     // init the buffer ready semaphore
00032     bufferReady = false;
00033     
00034     // Setup the SPI bus for 16 bit data, high steady state clock,
00035     // second edge capture, with a 1 MHz clock rate
00036     spi.format(16,3);
00037     spi.frequency(1000000);    
00038   
00039     // init the IMU
00040     InitImu();
00041     wait_us(100); // allow things to settle before starting interrupts   
00042     
00043     // Connect the IMU data-ready signal to its interrupt handler
00044     imuDataReady.fall(this, &ImuSpi::IMUDataReadyISR);
00045     
00046     // Connect the onePPS signal to its interrupt handler
00047     imuDataReady.rise(this, &ImuSpi::OnePPSISR);
00048   
00049 }
00050 
00051 
00052 // initialize the IMU
00053 void ImuSpi::InitImu(void) {
00054     // deselect the IMU
00055     cs = 1;
00056     reset = 1;
00057     // perform a hard reset
00058     reset = 0; wait(.1); reset = 1; wait(.1);   
00059     // send initialization commands
00060     WriteSynch(0x34, 0x04); // enable active low data ready on DIO1
00061     WriteSynch(0x39, 0x01); // Set the gyro sensitivity to 75 deg/sec
00062     WriteSynch(0x38, 0x00); // Set the FIR filter to its widest possible bandwidth
00063     
00064 }
00065 
00066 // When the IMU has data ready, trigger a burst read.
00067 // Launch an interrupt-driven process to read the IMU 
00068 // accelerometer and gyro registers into the pingpong buffer.
00069 void ImuSpi::IMUDataReadyISR(void) {
00070     // initialize the IMU data buffer
00071     wp = GetBufferWritePtr();
00072     imuCmdIndex = 0;
00073     imuReadIndex = 0;
00074     
00075     // reject spurious interrupts
00076     if (imuDataReady == 0) {      
00077         // set up the first register read operation
00078         SendReadCmd(imuRegs[imuCmdIndex++]);
00079     }
00080 
00081 }
00082 
00083 // Start an asynchronous (non-blocking) read of the IMU
00084 // register designated by 'adr'
00085 void ImuSpi::SendReadCmd(int8_t adr) {
00086     int16_t cmd = 0x3f00 & (((int16_t)adr) << 8);
00087     cs = 0;
00088     LPC_SSP0->DR = cmd;
00089     
00090     // start the timer that triggers the next command write
00091     writeTrigger.attach_us(this, &ImuSpi::WriteTriggerTimeoutISR, (16+9)); 
00092 
00093 }
00094 
00095 // handle the write trigger timeout interrupt
00096 void ImuSpi::WriteTriggerTimeoutISR(void) {
00097 diag = 1;
00098     // buffer the contents of the receive data register that was acquired 
00099     // during the prior command write
00100     *wp++ = (int16_t)(LPC_SSP0->DR);
00101     
00102     // if we've acquired an entire buffer of data, signal the foreground task
00103     if (imuReadIndex++ >= dataSize) {
00104         int16_t* p = GetBufferWritePtr();
00105         p[0] = sequenceNumber++; // buffer[0] has garbage, replace with the sequence number
00106         bufferReady = true;
00107         cs = 1; // deassert the IMU chip select
00108     } else {
00109         // we haven't yet read all the IMU registers, trigger the next read 
00110         SendReadCmd(imuRegs[imuCmdIndex++]);
00111     }    
00112 diag = 0;  
00113 }
00114 
00115 // The foreground process uses this method to determine if 
00116 // if a burst read of the IMU is complete. If read was 
00117 // completed, the ping-pong buffers are toggled so that the
00118 // most recent data is made available to the foreground
00119 // process.
00120 bool ImuSpi::IsBufferReady(void) {
00121     bool ret;
00122     
00123     if (bufferReady == true) {
00124         __disable_irq();    // ---- critical
00125         TogglePingpong();
00126         bufferReady = false;
00127         __enable_irq();     // ---- \critical
00128         ret = true;
00129     } else {
00130         ret = false;
00131     }
00132     
00133     return ret;
00134 }
00135 
00136 // perform a synchronous (blocking) read of the IMU
00137 // register designated by 'adr'
00138 int16_t ImuSpi::ReadSynch(char adr) {
00139     int16_t cmd = 0x3f00 & (((int16_t)adr) << 8);
00140     int16_t response;
00141     cs = 0;
00142     // (1) command a read of the desired register while reading back garbage
00143     spi.write(cmd);
00144     // (2) command a read of register 0x00 while reading back the desired register 
00145     response = spi.write(0x3f00); 
00146     cs = 1;
00147     return response;
00148 }
00149 
00150 // perform a synchronous (blocking) write of 'data' to the IMU
00151 // register designated by 'adr'
00152 void ImuSpi::WriteSynch(char adr, char data) {
00153     int16_t cmd = 0x8000 | (((int16_t)adr) << 8) | data;
00154     cs = 0;
00155     spi.write(cmd);
00156     cs = 1;
00157     wait_us(9);
00158 }
00159 
00160 // When the GPS 1 PPS signal is asserted, clear the sequence number.
00161 // This is how IMU data is synched with GPS data.
00162 void ImuSpi::OnePPSISR(void) {
00163     sequenceNumber = 0;
00164 }