Arduino as SPI slave, need low latency response

Hi,
I have a project in which I am using an Arduino Due as a motor controller (I want that to be running fast >= 10Khz), but I also need to be talking to sensors such as an I2C MPU6050 which takes ~6ms to communicate with. My current approach is to use a different Arduino (Pro Mini) to talk to the sensor and communicate with the Due using SPI.

I'm using the Due as SPI master and Pro Mini as slave, using "Slave (example) using interrupt for SS pin" on Gammon Forum : Electronics : Microprocessors : SPI - Serial Peripheral Interface - for Arduino as a guide. The Due initiates a communication, and the Pro Mini sends back desired motor positions to the Due through MISO.

However, it seems that the SPI interrupt is not high enough priority to interrupt any other work that the Pro Mini is doing (i.e. talking to the sensor).

Is there any way to make the slave suspend everything else it is doing and respond to the Due as fast as possible? Alternatively, is there any other sensible setup that would make sense in my use case?

Thanks!

Seeing the code on the Mini would be good. Interrupts should interrupt reading from the sensor. It may be that you are not processing the interrupt correctly.

A link to the I2C MPU6050 would be good, too. There may be a way to read the device asynchronously (i.e. non-blocking).

Thanks for the reply!

Here is my Pro Mini code:

// Written by Nick Gammon
// April 2011

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "pins_arduino.h"

#include "IMU.h"

#define DUE_INT 0
#define IMU_INT 1

IMU imu;

unsigned long niters = 0;
unsigned long tick_ms = millis();
int test_stage = 0;

// what to do with incoming data
volatile byte command;

volatile byte left_mode, right_mode;
volatile short left_val, right_val;
volatile int8_t tail_pitch, tail_yaw;

// start of transaction, no command yet
void ss_falling()
{
  command = 0;
}

void setup()
{
  Serial.begin(115200);
  // have to send on master in, *slave out*

  pinMode(MISO, OUTPUT);

  // turn on SPI in slave mode
  SPCR |= _BV(SPE);
  // turn on interrupts
  SPCR |= _BV(SPIE);

  // interrupt for SS falling edge
  attachInterrupt(DUE_INT, ss_falling, FALLING);

  // IMU
  imu.init(IMU_INT);
  
  // Timing-related stuff
  tick_ms = millis();
}


// SPI interrupt routine
ISR(SPI_STC_vect)
{
  byte c = SPDR;
  
  switch(command)
  {
  case 0:
    // left_mode
    SPDR = left_mode;
    break;

  case 1:
    // left_val msb
    SPDR = (left_val >> 8) & 0xff;
    break;

  case 2:
    // left_val lsb
    SPDR = left_val & 0xff;
    break;

  case 3:
    // right_mode
    SPDR = right_mode;
    break;

  case 4:
    // right_val msb
    SPDR = (right_val >> 8) & 0xff;
    break;

  case 5:
    // right_val lsb
    SPDR = right_val & 0xff;
    break;

  case 6:
    // tail_pitch
    SPDR = tail_pitch;
    break;

  case 7:
    // tail_yaw
    SPDR = tail_yaw;
    break;
    
  default:
    // Use this to sync! the first good byte (if sensible) should 
    // only be 0 or 1
    SPDR = 123;
  }

  command++;
}


void loop()
{
  //IMU
  imu.update(); // THIS IS WHAT "BLOCKS"
  
  // Every xx ms
  niters++;
  if (millis() - tick_ms > 5000)
  {
    unsigned long current_ms = millis();

    // Setting left_mode, left_val etc.
}

The datasheet for the IMU is here http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf, and in particular I am using the i2cdevlib library i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub The fact that it is blocking is just because the data packet it sends over I2C is large and I can't see a way around that.

I would like to learn how to interrupt the sensor readings. Should the SPI code go in the "ss_falling" ISR? If so, how would it look? (I'm not very good with AVR and/or SPI, apologies, the extent of my knowledge is Gammon Forum : Electronics : Microprocessors : SPI - Serial Peripheral Interface - for Arduino)

Thanks!

Where is the IMU.h from? Provide a link to that library. You don't seem to use the I2Cdev library though.

Oh, sorry. Here it is

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
  mpuInterrupt = true;
}

class IMU
{
public:
  int interrupt_num;
  float yaw, pitch, roll;
  MPU6050 mpu;

  void init(int interrupt_num)
  {
    this->interrupt_num = interrupt_num;
    
    dmpReady = false;

    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    // initialize device
    mpu.initialize();

    // verify connection
    //Serial.println("Testing device connections...");
    if (!mpu.testConnection())
    {
      Serial.println("MPU6050 connection failed.");
    }
    else
      Serial.println("MPU6050 connection successful.");

    // load and configure the DMP
    //Serial.println("Initializing DMP...");
    devStatus = mpu.dmpInitialize();

    // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
      // turn on the DMP, now that it's ready
      //Serial.println("Enabling DMP...");
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      //Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
      attachInterrupt(interrupt_num, dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      //Serial.println("DMP ready! Waiting for first interrupt...");
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
    } 
    else
    {
      // ERROR!
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)
      Serial.print("DMP Initialization failed (code ");
      Serial.print(devStatus);
      Serial.println(")");
    }

    // Set offsets (use print_biases() to fix)
//    mpu.setXAccelOffset(-835);
//    mpu.setYAccelOffset(1426);
//    mpu.setZAccelOffset(2164);
//    mpu.setXGyroOffset(43);
//    mpu.setYGyroOffset(32);
//    mpu.setZGyroOffset(15);
  }

  void print_biases()
  {
    long tax = 0, tay = 0, taz = 0, tgx = 0, tgy = 0, tgz = 0;
    const int n_offs_samples = 500;
    for (int i=0; i<n_offs_samples; ++i)
    {
      mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      tax += ax;
      tay += ay;
      taz += az;
      tgx += gx;
      tgy += gy;
      tgz += gz;
    }
    tax /= n_offs_samples;
    tay /= n_offs_samples;
    taz /= n_offs_samples;
    tgx /= n_offs_samples;
    tgy /= n_offs_samples;
    tgz /= n_offs_samples;
    Serial.print(tax);
    Serial.print("\t");
    Serial.print(tay);
    Serial.print("\t");
    Serial.print(taz);
    Serial.print("\t");
    Serial.print(tgx);
    Serial.print("\t");
    Serial.print(tgy);
    Serial.print("\t");
    Serial.println(tgz);
  }

  // UPDATE IMU STATE -- CALL ONCE PER LOOP
  void update()
  {
    // Check if interrupt or unprocessed packets are available
    if (mpuInterrupt || fifoCount >= packetSize) 
    {
      // reset interrupt flag and get INT_STATUS byte
      mpuInterrupt = false;
      mpuIntStatus = mpu.getIntStatus();

      // get current FIFO count
      fifoCount = mpu.getFIFOCount();

      // check for overflow (this should never happen unless our code is too inefficient)
      if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        //Serial.println("FIFO overflow!");

        // otherwise, check for DMP data ready interrupt (this should happen frequently)
      } 
      else if (mpuIntStatus & 0x02)
      {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);

        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        yaw = ypr[0];
        pitch = ypr[2];
        roll = -ypr[1];
        Serial.print(yaw * 180/M_PI);
        Serial.print("\t");
        Serial.print(pitch * 180/M_PI);
        Serial.print("\t");
        Serial.println(roll * 180/M_PI);
      }
    }
  }

protected:
  // MPU control/status vars
  bool dmpReady;  // set true if DMP init was successful
  uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  uint16_t fifoCount;     // count of all bytes currently in FIFO
  uint8_t fifoBuffer[64]; // FIFO storage buffer

  // orientation/motion vars
  Quaternion q;           // [w, x, y, z]         quaternion container
  VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  VectorFloat gravity;    // [x, y, z]            gravity vector
  float euler[3];         // [psi, theta, phi]    Euler angle container
  float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

  int16_t ax, ay, az;
  int16_t gx, gy, gz;
};

It’s copied from the DMP example in i2cdevlib.

I should point out that, if I leave the MPU6050 interrupt on, but don’t call “imu.update()” in the “loop()” function (see previous post), then things work fine. It’s only the code that is run in “imu.update()” which is causing slow-downs, and I want the SPI interrupt to get priority over that.

Thanks!

I want the SPI interrupt to get priority over that.

The SPI interrupt should always have priority over that. The only way to change that is by disabling interrupts (with cli()) or if a lengthy interrupt handler is blocking other interrupts. How do you measure the response time?

Hi pylon,
You were correct! I assumed (because my motor tracking was jumpy) that it was because of uneven timesteps, but it was actually because I was getting garbage from SPI. I kind of solved this by increasing the delay between calling SPI.transfer from the Due and using the read back byte, as well as ultimately using a stupid checking scheme by sending the same data twice. It makes my control loop much slower but more reliable.

PS. I think some of these problems are due to EMI, and so the ultimate fix would be to use RS485 or some other differential signal. For now I'm using short cables.