This program acquires data from an ADIS16355 IMU via the SPI bus and sends the data to a host computer via UDP over Ethernet.
Dependencies: mbed StrippedDownNetServices
imu-spi.cpp@0:79f663186c05, 2012-05-31 (annotated)
- Committer:
- yahugh
- Date:
- Thu May 31 18:37:12 2012 +0000
- Revision:
- 0:79f663186c05
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yahugh | 0:79f663186c05 | 1 | // |
yahugh | 0:79f663186c05 | 2 | // imu-spi.cpp |
yahugh | 0:79f663186c05 | 3 | // |
yahugh | 0:79f663186c05 | 4 | // copyright 2010 Hugh Shane |
yahugh | 0:79f663186c05 | 5 | // |
yahugh | 0:79f663186c05 | 6 | #include "mbed.h" |
yahugh | 0:79f663186c05 | 7 | #include "imu-spi.h" |
yahugh | 0:79f663186c05 | 8 | |
yahugh | 0:79f663186c05 | 9 | ImuSpi::ImuSpi(void) : |
yahugh | 0:79f663186c05 | 10 | spi(p11, p12, p13), // mosi, miso, sclk (Note: mbed uses SSPx for the SPI busses) |
yahugh | 0:79f663186c05 | 11 | cs(p16), // IMU chip select, active low |
yahugh | 0:79f663186c05 | 12 | reset(p15), // IMU reset, active low. Also resets the altimeter. |
yahugh | 0:79f663186c05 | 13 | diag(p17), // diagnostic output |
yahugh | 0:79f663186c05 | 14 | imuDataReady(p14), // IMU data-ready interrupt input, active falling edge |
yahugh | 0:79f663186c05 | 15 | onePPS(p19) // GPS 1 PPS interrupt input, active rising edge |
yahugh | 0:79f663186c05 | 16 | { |
yahugh | 0:79f663186c05 | 17 | cs = 1; // set to quiescent state |
yahugh | 0:79f663186c05 | 18 | diag = 0; |
yahugh | 0:79f663186c05 | 19 | imuDataReady.mode(PullUp); // just to be safe |
yahugh | 0:79f663186c05 | 20 | sequenceNumber = 0; |
yahugh | 0:79f663186c05 | 21 | imuReadIndex = 0; |
yahugh | 0:79f663186c05 | 22 | |
yahugh | 0:79f663186c05 | 23 | // Initialize the IMU command sequence. Note: register 0x00 is a dummy |
yahugh | 0:79f663186c05 | 24 | // value that triggers the last read in a burst cycle. |
yahugh | 0:79f663186c05 | 25 | int8_t regVals[] = {0x04,0x06,0x08,0x0A,0x0C,0x0E,0x3C,0x00}; |
yahugh | 0:79f663186c05 | 26 | for (int i = 0; i < bufferSize; i++) imuRegs[i] = regVals[i]; |
yahugh | 0:79f663186c05 | 27 | |
yahugh | 0:79f663186c05 | 28 | // init the pingpong buffer |
yahugh | 0:79f663186c05 | 29 | pingpong = 0; |
yahugh | 0:79f663186c05 | 30 | |
yahugh | 0:79f663186c05 | 31 | // init the buffer ready semaphore |
yahugh | 0:79f663186c05 | 32 | bufferReady = false; |
yahugh | 0:79f663186c05 | 33 | |
yahugh | 0:79f663186c05 | 34 | // Setup the SPI bus for 16 bit data, high steady state clock, |
yahugh | 0:79f663186c05 | 35 | // second edge capture, with a 1 MHz clock rate |
yahugh | 0:79f663186c05 | 36 | spi.format(16,3); |
yahugh | 0:79f663186c05 | 37 | spi.frequency(1000000); |
yahugh | 0:79f663186c05 | 38 | |
yahugh | 0:79f663186c05 | 39 | // init the IMU |
yahugh | 0:79f663186c05 | 40 | InitImu(); |
yahugh | 0:79f663186c05 | 41 | wait_us(100); // allow things to settle before starting interrupts |
yahugh | 0:79f663186c05 | 42 | |
yahugh | 0:79f663186c05 | 43 | // Connect the IMU data-ready signal to its interrupt handler |
yahugh | 0:79f663186c05 | 44 | imuDataReady.fall(this, &ImuSpi::IMUDataReadyISR); |
yahugh | 0:79f663186c05 | 45 | |
yahugh | 0:79f663186c05 | 46 | // Connect the onePPS signal to its interrupt handler |
yahugh | 0:79f663186c05 | 47 | imuDataReady.rise(this, &ImuSpi::OnePPSISR); |
yahugh | 0:79f663186c05 | 48 | |
yahugh | 0:79f663186c05 | 49 | } |
yahugh | 0:79f663186c05 | 50 | |
yahugh | 0:79f663186c05 | 51 | |
yahugh | 0:79f663186c05 | 52 | // initialize the IMU |
yahugh | 0:79f663186c05 | 53 | void ImuSpi::InitImu(void) { |
yahugh | 0:79f663186c05 | 54 | // deselect the IMU |
yahugh | 0:79f663186c05 | 55 | cs = 1; |
yahugh | 0:79f663186c05 | 56 | reset = 1; |
yahugh | 0:79f663186c05 | 57 | // perform a hard reset |
yahugh | 0:79f663186c05 | 58 | reset = 0; wait(.1); reset = 1; wait(.1); |
yahugh | 0:79f663186c05 | 59 | // send initialization commands |
yahugh | 0:79f663186c05 | 60 | WriteSynch(0x34, 0x04); // enable active low data ready on DIO1 |
yahugh | 0:79f663186c05 | 61 | WriteSynch(0x39, 0x01); // Set the gyro sensitivity to 75 deg/sec |
yahugh | 0:79f663186c05 | 62 | WriteSynch(0x38, 0x00); // Set the FIR filter to its widest possible bandwidth |
yahugh | 0:79f663186c05 | 63 | |
yahugh | 0:79f663186c05 | 64 | } |
yahugh | 0:79f663186c05 | 65 | |
yahugh | 0:79f663186c05 | 66 | // When the IMU has data ready, trigger a burst read. |
yahugh | 0:79f663186c05 | 67 | // Launch an interrupt-driven process to read the IMU |
yahugh | 0:79f663186c05 | 68 | // accelerometer and gyro registers into the pingpong buffer. |
yahugh | 0:79f663186c05 | 69 | void ImuSpi::IMUDataReadyISR(void) { |
yahugh | 0:79f663186c05 | 70 | // initialize the IMU data buffer |
yahugh | 0:79f663186c05 | 71 | wp = GetBufferWritePtr(); |
yahugh | 0:79f663186c05 | 72 | imuCmdIndex = 0; |
yahugh | 0:79f663186c05 | 73 | imuReadIndex = 0; |
yahugh | 0:79f663186c05 | 74 | |
yahugh | 0:79f663186c05 | 75 | // reject spurious interrupts |
yahugh | 0:79f663186c05 | 76 | if (imuDataReady == 0) { |
yahugh | 0:79f663186c05 | 77 | // set up the first register read operation |
yahugh | 0:79f663186c05 | 78 | SendReadCmd(imuRegs[imuCmdIndex++]); |
yahugh | 0:79f663186c05 | 79 | } |
yahugh | 0:79f663186c05 | 80 | |
yahugh | 0:79f663186c05 | 81 | } |
yahugh | 0:79f663186c05 | 82 | |
yahugh | 0:79f663186c05 | 83 | // Start an asynchronous (non-blocking) read of the IMU |
yahugh | 0:79f663186c05 | 84 | // register designated by 'adr' |
yahugh | 0:79f663186c05 | 85 | void ImuSpi::SendReadCmd(int8_t adr) { |
yahugh | 0:79f663186c05 | 86 | int16_t cmd = 0x3f00 & (((int16_t)adr) << 8); |
yahugh | 0:79f663186c05 | 87 | cs = 0; |
yahugh | 0:79f663186c05 | 88 | LPC_SSP0->DR = cmd; |
yahugh | 0:79f663186c05 | 89 | |
yahugh | 0:79f663186c05 | 90 | // start the timer that triggers the next command write |
yahugh | 0:79f663186c05 | 91 | writeTrigger.attach_us(this, &ImuSpi::WriteTriggerTimeoutISR, (16+9)); |
yahugh | 0:79f663186c05 | 92 | |
yahugh | 0:79f663186c05 | 93 | } |
yahugh | 0:79f663186c05 | 94 | |
yahugh | 0:79f663186c05 | 95 | // handle the write trigger timeout interrupt |
yahugh | 0:79f663186c05 | 96 | void ImuSpi::WriteTriggerTimeoutISR(void) { |
yahugh | 0:79f663186c05 | 97 | diag = 1; |
yahugh | 0:79f663186c05 | 98 | // buffer the contents of the receive data register that was acquired |
yahugh | 0:79f663186c05 | 99 | // during the prior command write |
yahugh | 0:79f663186c05 | 100 | *wp++ = (int16_t)(LPC_SSP0->DR); |
yahugh | 0:79f663186c05 | 101 | |
yahugh | 0:79f663186c05 | 102 | // if we've acquired an entire buffer of data, signal the foreground task |
yahugh | 0:79f663186c05 | 103 | if (imuReadIndex++ >= dataSize) { |
yahugh | 0:79f663186c05 | 104 | int16_t* p = GetBufferWritePtr(); |
yahugh | 0:79f663186c05 | 105 | p[0] = sequenceNumber++; // buffer[0] has garbage, replace with the sequence number |
yahugh | 0:79f663186c05 | 106 | bufferReady = true; |
yahugh | 0:79f663186c05 | 107 | cs = 1; // deassert the IMU chip select |
yahugh | 0:79f663186c05 | 108 | } else { |
yahugh | 0:79f663186c05 | 109 | // we haven't yet read all the IMU registers, trigger the next read |
yahugh | 0:79f663186c05 | 110 | SendReadCmd(imuRegs[imuCmdIndex++]); |
yahugh | 0:79f663186c05 | 111 | } |
yahugh | 0:79f663186c05 | 112 | diag = 0; |
yahugh | 0:79f663186c05 | 113 | } |
yahugh | 0:79f663186c05 | 114 | |
yahugh | 0:79f663186c05 | 115 | // The foreground process uses this method to determine if |
yahugh | 0:79f663186c05 | 116 | // if a burst read of the IMU is complete. If read was |
yahugh | 0:79f663186c05 | 117 | // completed, the ping-pong buffers are toggled so that the |
yahugh | 0:79f663186c05 | 118 | // most recent data is made available to the foreground |
yahugh | 0:79f663186c05 | 119 | // process. |
yahugh | 0:79f663186c05 | 120 | bool ImuSpi::IsBufferReady(void) { |
yahugh | 0:79f663186c05 | 121 | bool ret; |
yahugh | 0:79f663186c05 | 122 | |
yahugh | 0:79f663186c05 | 123 | if (bufferReady == true) { |
yahugh | 0:79f663186c05 | 124 | __disable_irq(); // ---- critical |
yahugh | 0:79f663186c05 | 125 | TogglePingpong(); |
yahugh | 0:79f663186c05 | 126 | bufferReady = false; |
yahugh | 0:79f663186c05 | 127 | __enable_irq(); // ---- \critical |
yahugh | 0:79f663186c05 | 128 | ret = true; |
yahugh | 0:79f663186c05 | 129 | } else { |
yahugh | 0:79f663186c05 | 130 | ret = false; |
yahugh | 0:79f663186c05 | 131 | } |
yahugh | 0:79f663186c05 | 132 | |
yahugh | 0:79f663186c05 | 133 | return ret; |
yahugh | 0:79f663186c05 | 134 | } |
yahugh | 0:79f663186c05 | 135 | |
yahugh | 0:79f663186c05 | 136 | // perform a synchronous (blocking) read of the IMU |
yahugh | 0:79f663186c05 | 137 | // register designated by 'adr' |
yahugh | 0:79f663186c05 | 138 | int16_t ImuSpi::ReadSynch(char adr) { |
yahugh | 0:79f663186c05 | 139 | int16_t cmd = 0x3f00 & (((int16_t)adr) << 8); |
yahugh | 0:79f663186c05 | 140 | int16_t response; |
yahugh | 0:79f663186c05 | 141 | cs = 0; |
yahugh | 0:79f663186c05 | 142 | // (1) command a read of the desired register while reading back garbage |
yahugh | 0:79f663186c05 | 143 | spi.write(cmd); |
yahugh | 0:79f663186c05 | 144 | // (2) command a read of register 0x00 while reading back the desired register |
yahugh | 0:79f663186c05 | 145 | response = spi.write(0x3f00); |
yahugh | 0:79f663186c05 | 146 | cs = 1; |
yahugh | 0:79f663186c05 | 147 | return response; |
yahugh | 0:79f663186c05 | 148 | } |
yahugh | 0:79f663186c05 | 149 | |
yahugh | 0:79f663186c05 | 150 | // perform a synchronous (blocking) write of 'data' to the IMU |
yahugh | 0:79f663186c05 | 151 | // register designated by 'adr' |
yahugh | 0:79f663186c05 | 152 | void ImuSpi::WriteSynch(char adr, char data) { |
yahugh | 0:79f663186c05 | 153 | int16_t cmd = 0x8000 | (((int16_t)adr) << 8) | data; |
yahugh | 0:79f663186c05 | 154 | cs = 0; |
yahugh | 0:79f663186c05 | 155 | spi.write(cmd); |
yahugh | 0:79f663186c05 | 156 | cs = 1; |
yahugh | 0:79f663186c05 | 157 | wait_us(9); |
yahugh | 0:79f663186c05 | 158 | } |
yahugh | 0:79f663186c05 | 159 | |
yahugh | 0:79f663186c05 | 160 | // When the GPS 1 PPS signal is asserted, clear the sequence number. |
yahugh | 0:79f663186c05 | 161 | // This is how IMU data is synched with GPS data. |
yahugh | 0:79f663186c05 | 162 | void ImuSpi::OnePPSISR(void) { |
yahugh | 0:79f663186c05 | 163 | sequenceNumber = 0; |
yahugh | 0:79f663186c05 | 164 | } |