Diff: imu-spi.cpp

Revision:

0:79f663186c05

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/imu-spi.cpp	Thu May 31 18:37:12 2012 +0000
@@ -0,0 +1,164 @@
+//
+// imu-spi.cpp
+//
+// copyright 2010 Hugh Shane
+//
+#include "mbed.h"
+#include "imu-spi.h"
+
+ImuSpi::ImuSpi(void) :
+    spi(p11, p12, p13), // mosi, miso, sclk (Note: mbed uses SSPx for the SPI busses)
+    cs(p16),            // IMU chip select, active low
+    reset(p15),         // IMU reset, active low. Also resets the altimeter.
+    diag(p17),          // diagnostic output
+    imuDataReady(p14),  // IMU data-ready interrupt input, active falling edge
+    onePPS(p19)         // GPS 1 PPS interrupt input, active rising edge
+{
+    cs = 1; // set to quiescent state
+    diag = 0;
+    imuDataReady.mode(PullUp); // just to be safe
+    sequenceNumber = 0;
+    imuReadIndex = 0;
+
+    // Initialize the IMU command sequence. Note: register 0x00 is a dummy 
+    // value that triggers the last read in a burst cycle.
+    int8_t regVals[] = {0x04,0x06,0x08,0x0A,0x0C,0x0E,0x3C,0x00};
+    for (int i = 0; i < bufferSize; i++) imuRegs[i] = regVals[i];
+    
+    // init the pingpong buffer
+    pingpong = 0;
+    
+    // init the buffer ready semaphore
+    bufferReady = false;
+    
+    // Setup the SPI bus for 16 bit data, high steady state clock,
+    // second edge capture, with a 1 MHz clock rate
+    spi.format(16,3);
+    spi.frequency(1000000);    
+  
+    // init the IMU
+    InitImu();
+    wait_us(100); // allow things to settle before starting interrupts   
+    
+    // Connect the IMU data-ready signal to its interrupt handler
+    imuDataReady.fall(this, &ImuSpi::IMUDataReadyISR);
+    
+    // Connect the onePPS signal to its interrupt handler
+    imuDataReady.rise(this, &ImuSpi::OnePPSISR);
+  
+}
+
+
+// initialize the IMU
+void ImuSpi::InitImu(void) {
+    // deselect the IMU
+    cs = 1;
+    reset = 1;
+    // perform a hard reset
+    reset = 0; wait(.1); reset = 1; wait(.1);   
+    // send initialization commands
+    WriteSynch(0x34, 0x04); // enable active low data ready on DIO1
+    WriteSynch(0x39, 0x01); // Set the gyro sensitivity to 75 deg/sec
+    WriteSynch(0x38, 0x00); // Set the FIR filter to its widest possible bandwidth
+    
+}
+
+// When the IMU has data ready, trigger a burst read.
+// Launch an interrupt-driven process to read the IMU 
+// accelerometer and gyro registers into the pingpong buffer.
+void ImuSpi::IMUDataReadyISR(void) {
+    // initialize the IMU data buffer
+    wp = GetBufferWritePtr();
+    imuCmdIndex = 0;
+    imuReadIndex = 0;
+    
+    // reject spurious interrupts
+    if (imuDataReady == 0) {      
+        // set up the first register read operation
+        SendReadCmd(imuRegs[imuCmdIndex++]);
+    }
+
+}
+
+// Start an asynchronous (non-blocking) read of the IMU
+// register designated by 'adr'
+void ImuSpi::SendReadCmd(int8_t adr) {
+    int16_t cmd = 0x3f00 & (((int16_t)adr) << 8);
+    cs = 0;
+    LPC_SSP0->DR = cmd;
+    
+    // start the timer that triggers the next command write
+    writeTrigger.attach_us(this, &ImuSpi::WriteTriggerTimeoutISR, (16+9)); 
+
+}
+
+// handle the write trigger timeout interrupt
+void ImuSpi::WriteTriggerTimeoutISR(void) {
+diag = 1;
+    // buffer the contents of the receive data register that was acquired 
+    // during the prior command write
+    *wp++ = (int16_t)(LPC_SSP0->DR);
+    
+    // if we've acquired an entire buffer of data, signal the foreground task
+    if (imuReadIndex++ >= dataSize) {
+        int16_t* p = GetBufferWritePtr();
+        p[0] = sequenceNumber++; // buffer[0] has garbage, replace with the sequence number
+        bufferReady = true;
+        cs = 1; // deassert the IMU chip select
+    } else {
+        // we haven't yet read all the IMU registers, trigger the next read 
+        SendReadCmd(imuRegs[imuCmdIndex++]);
+    }    
+diag = 0;  
+}
+
+// The foreground process uses this method to determine if 
+// if a burst read of the IMU is complete. If read was 
+// completed, the ping-pong buffers are toggled so that the
+// most recent data is made available to the foreground
+// process.
+bool ImuSpi::IsBufferReady(void) {
+    bool ret;
+    
+    if (bufferReady == true) {
+        __disable_irq();    // ---- critical
+        TogglePingpong();
+        bufferReady = false;
+        __enable_irq();     // ---- \critical
+        ret = true;
+    } else {
+        ret = false;
+    }
+    
+    return ret;
+}
+
+// perform a synchronous (blocking) read of the IMU
+// register designated by 'adr'
+int16_t ImuSpi::ReadSynch(char adr) {
+    int16_t cmd = 0x3f00 & (((int16_t)adr) << 8);
+    int16_t response;
+    cs = 0;
+    // (1) command a read of the desired register while reading back garbage
+    spi.write(cmd);
+    // (2) command a read of register 0x00 while reading back the desired register 
+    response = spi.write(0x3f00); 
+    cs = 1;
+    return response;
+}
+
+// perform a synchronous (blocking) write of 'data' to the IMU
+// register designated by 'adr'
+void ImuSpi::WriteSynch(char adr, char data) {
+    int16_t cmd = 0x8000 | (((int16_t)adr) << 8) | data;
+    cs = 0;
+    spi.write(cmd);
+    cs = 1;
+    wait_us(9);
+}
+
+// When the GPS 1 PPS signal is asserted, clear the sequence number.
+// This is how IMU data is synched with GPS data.
+void ImuSpi::OnePPSISR(void) {
+    sequenceNumber = 0;
+}
\ No newline at end of file