IMU and GPS query with due timer interrupt

Dear Arduino community,

I would like to create a GPS and IMU data logger. I am using Arduino Duemilanove board, Ublox Neo 6M GPS and a GY-87 IMU chip.

I used the following main references to create my code:
[1] http://ko7m.blogspot.hu/2015/01/arduino-due-timers-part-1.html
[2] Adafruit_GPS library due-parsing example code

In my code I am using due timer interrupts for uniform sampling and buffers to store the IMU and GPS data into SD card. But the GPS code segment not complete, because I can not get correct NMEA sentences.

My problem is that, when I set simultaneously turn on the GPS and the IMU in the timer interrupt, there will be missing characters in the NMEA sentences, and I can not get fix GPS data at all. However when I turn off the IMU code segment I get correct GPS sentences with fix GPS data as in the due_parsing example code.

I tried various timer frequency but I experienced the same effect.

Could anyone tell me what could be cause this effect and how could be solve it?
Am I using to many codeline in the timer interrupt? Maybe I have to use two timer interrupts one for GPS and one for IMU query?

Any help or hints greatly appreciated.

Here is the simplified version of my code:

#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GPS.h> 

#define MPU6050_ADDRESS 0x68
#define HMC5883L_ADDRESS 0x1E
#define SMPLRT_DIV 0x19
#define ACC_CONFIG 0x1C
#define GYR_CONFIG 0x1B
#define MGN_CONFIG 0x01

uint16_t parseFreqGPS = 2048;
uint16_t parseFreqIMU = 256;
uint16_t selecter = round((float)parseFreqGPS/(float)parseFreqIMU);

#define mySerial Serial1
Adafruit_GPS GPS(&mySerial);
char oneChar;

const uint16_t metaNumber = 3;
uint32_t metaData[metaNumber+2];
const uint16_t quanNumber = 10;

char filledBuffer; // A = saving buffer A, B = saving buffer B, X = non of them
uint16_t bufferIncrnt = 0; // 
const uint16_t bufMaxIncrnt = 36;
uint32_t bufferChecker[2]; // For to check there is no missing data during the buffer loading
uint16_t missedCounter = 0;  // Number of the outstages when data loss occour

const uint16_t bufferLngthM = metaNumber * bufMaxIncrnt;
uint32_t metaBufferA[bufferLngthM];
uint32_t metaBufferB[bufferLngthM];
volatile uint32_t *metaLoadPr;
volatile uint32_t *metaSavePr;

const uint16_t bufferLngthQ = quanNumber * bufMaxIncrnt;
uint16_t quanBufferA[bufferLngthQ];
uint16_t quanBufferB[bufferLngthQ];
uint16_t *quanLoadPr;
uint16_t *quanSavePr;

void setup() {

  Serial.begin(115200);
  while (!Serial) {;}
  
  parseFreqGPS = parseFreqGPS>>1; // Hence, I get the correct freq during operation.

  // Initialization of GY-87 IMU
  Wire.begin();
  setupIMU();
  
  //Initialization of Ublox NEO-6M-0-001 GPS
  GPS.begin(9600);
  mySerial.begin(9600);
  GPS.sendCommand("$PUBX,40,GLL,0,0,0,0*5C\r\n"); // Turn of GLL sentences of Ublox
  GPS.sendCommand("$PUBX,40,ZDA,0,0,0,0*44\r\n"); // Turn of ZDA sentences of Ublox
  GPS.sendCommand("$PUBX,40,VTG,0,0,0,0*5E\r\n"); // Turn of VTG sentences of Ublox
  GPS.sendCommand("$PUBX,40,GSV,0,0,0,0*59\r\n"); // Turn of GSV sentences of Ublox
  GPS.sendCommand("$PUBX,40,GSA,0,0,0,0*4E\r\n"); // Turn of GSA sentences of Ublox
  delay(500); // For the GPS
  
  for (int i = 0; i < sizeof(metaData)/sizeof(metaData[0]); i++) {
    metaData[i] = 0;
  }
  for (int i = 0; i < bufferLngthM; i++) {
    metaBufferA[i] = 0;
    metaBufferB[i] = 0;
  }
  for (int i = 0; i < bufferLngthQ; i++) {
    quanBufferA[i] = 0;
    quanBufferB[i] = 0;
  }
  metaLoadPr = metaBufferA;
  quanLoadPr = quanBufferA;
  filledBuffer = 'X';
  bufferIncrnt = 0;
  startTimer(TC1, 0, TC3_IRQn, parseFreqGPS);
  metaData[3] = micros();
}

void loop() {
  
  if ( (filledBuffer == 'A') || (filledBuffer == 'B') ) {
    Serial.print(F(" Buffer: ")); Serial.println(filledBuffer);
    bufferChecker[0] = metaSavePr[0]; // For checking the optimal buffer size
    /*for (int i=0; i < bufMaxIncrnt; i++) { // To store the data
      Serial.print( metaSavePr[i*metaNumber] );
      for (int j=1; j < metaNumber; j++) {
        Serial.print(';');
        Serial.print( metaSavePr[i*metaNumber+j] );
      }
      for (int j=0; j < quanNumber; j++) {
        Serial.print(';');
        Serial.print( (int16_t ) quanSavePr[i*quanNumber+j] );
      }
      Serial.println();
    }*/
    bufferChecker[1] = metaSavePr[(bufMaxIncrnt-1)*metaNumber]; // For checking the optimal buffer size
    if ( (bufferChecker[1]-(bufMaxIncrnt-1) ) != bufferChecker[0] ) {
      missedCounter++;
      Serial.print(F("Missed data record: "));
      Serial.print( (uint32_t)(bufferChecker[1] - (bufMaxIncrnt-1) - bufferChecker[0]) );
    } else {
      Serial.print(F("No data loss."));
    }
    filledBuffer = 'X';
  }
  
  if (GPS.newNMEAreceived()) {
    Serial.println(GPS.lastNMEA());
    if (GPS.parse(GPS.lastNMEA())) {
      Serial.print("\nTime: ");
      Serial.print(GPS.time, DEC); Serial.println();
      Serial.print("Fix: "); Serial.print((int)GPS.fix);
      Serial.print(" Quality: "); Serial.println((int)GPS.fixquality);
    }
  }
}

void TC3_Handler() {
  TC_GetStatus(TC1, 0);
  metaData[4]++; // Count the measurements
  oneChar = GPS.read(); // set to comment line if you would like turn off
  
  if ( (metaData[4]%selecter) == 0 ) { // set to -1 if you would like to turn off
    metaData[0]++; // Count the measurements
    metaData[1] = micros(); // Current Time in usec
    metaData[2] = metaData[1] - metaData[3]; // Sampling interval
    metaData[3] = metaData[1]; // Previous Time
    for (int i = 0; i < metaNumber; i++) { metaLoadPr[metaNumber*bufferIncrnt+i] = metaData[i]; }
    getIMUValues( quanLoadPr, quanNumber*bufferIncrnt ); // Request IMU readings
    bufferIncrnt++;
    if (bufferIncrnt == bufMaxIncrnt) {
      if (metaLoadPr == metaBufferA) {
        metaSavePr = metaBufferA;
        quanSavePr = quanBufferA;
        filledBuffer = 'A';
        metaLoadPr = metaBufferB;
        quanLoadPr = quanBufferB;
      } else {
        metaSavePr = metaBufferB;
        quanSavePr = quanBufferB;
        filledBuffer = 'B';
        metaLoadPr = metaBufferA;
        quanLoadPr = quanBufferA;
      }
      bufferIncrnt = 0;
    }
  }
}

void startTimer(Tc *tc, uint32_t channel, IRQn_Type irq, uint32_t freq) {
   pmc_set_writeprotect(false);
   pmc_enable_periph_clk(irq);
   TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK4);
   uint32_t rc = VARIANT_MCK / 128 / freq ;
   TC_SetRA(tc, channel, rc >> 1); // 50% duty cycle square wave
   TC_SetRC(tc, channel, rc);
   TC_Start(tc, channel);
   tc->TC_CHANNEL[channel].TC_IER=  TC_IER_CPCS | TC_IER_CPAS;
   tc->TC_CHANNEL[channel].TC_IDR=~(TC_IER_CPCS | TC_IER_CPAS);
   NVIC_EnableIRQ(irq);
}

void writeRegister(int deviceAddress, byte address, byte val) {
  Wire.beginTransmission(deviceAddress);
  Wire.write(address);
  Wire.write(val);
  Wire.endTransmission();
}

void getIMUValues(uint16_t *&quanPr, uint16_t inc ) {
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS,14,true);
  quanPr[inc+0] = Wire.read()<<8|Wire.read();
  quanPr[inc+1] = Wire.read()<<8|Wire.read();
  quanPr[inc+2] = Wire.read()<<8|Wire.read();
  quanPr[inc+3] = Wire.read()<<8|Wire.read();
  quanPr[inc+4] = Wire.read()<<8|Wire.read();
  quanPr[inc+5] = Wire.read()<<8|Wire.read();
  quanPr[inc+6] = Wire.read()<<8|Wire.read();
  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x03);
  Wire.endTransmission(false);
  Wire.requestFrom(HMC5883L_ADDRESS,6,true);
  quanPr[inc+7] = Wire.read()<<8|Wire.read();
  quanPr[inc+8] = Wire.read()<<8|Wire.read();
  quanPr[inc+9] = Wire.read()<<8|Wire.read();    
}

void setupIMU() {
  writeRegister(MPU6050_ADDRESS, 0x37, 0x02);
  writeRegister(MPU6050_ADDRESS, 0x6A, 0x00);
  writeRegister(MPU6050_ADDRESS, 0x6B, 0x00);
  writeRegister(HMC5883L_ADDRESS, 0x02, 0x00);
  writeRegister(MPU6050_ADDRESS, SMPLRT_DIV, 0x07);
  writeRegister(MPU6050_ADDRESS, ACC_CONFIG, 0 << 3);
  writeRegister(MPU6050_ADDRESS, GYR_CONFIG, 0 << 3);
  writeRegister(MPU6050_ADDRESS, MGN_CONFIG, 1 << 5);
}

You should be collecting GPS data on every pass through loop(). You should be collecting IMU data on some passes.

In the timer interrupt (uselessly complex), you set a flag. On the next pass through loop(), when you see the flag set, write the latest data to the SD card, and clear the flag.

There really is no reason to use a timer interrupt to tell that n milliseconds has passed since you last wrote, since a call to millis() will give you the same information.

If I collect data inside the loop, I think I will get non-uniformly sampling data, which will be depend on the SD card writing speed. That is why I am using timer interrupt. Or do I correct?

I think I will get non-uniformly sampling data

How do you think you are going to get uniformly spaced samples from the GPS, when it sends data when it is darned good and ready? Why is uniform spacing of GPS data important? The accuracy of the data is typically +/- 10 meters. Whether a sample is a few milliseconds early or late is not going to affect that accuracy.

Yes I was not precise.

I would like to store IMU data with 256 Hz and GPS data with 1 Hz (if it is available) to the SD card. (In the Adafruit_GPS due_parsing example the update rate of GPS is set to 1 Hz.) If fresh fix GPS data not available then store a Not a Number value or something else to denote there was no GPS data.

The implementation concept in my code is would be the same as like in [1] entry 9. ( Fill a buffer "A" with IMU data, while the content of buffer "B", which was previously filled, printed out to the SD card. If the buffer "A" is filled, swap with buffer "B". Choose the size of buffer "B" and "A" such a way that filling time always greater than the writing time. Buffer filling is carried out with timer interrupt, while SD writing is executed inside the main loop)

In my code inside the interrupt I put the GPS query code line oneChar = GPS.read(); from the due_parsing example code.

At the first round I would like to get only the nmea sentences in the main loop: if (GPS.newNMEAreceived()) { ** Serial.println(GPS.lastNMEA());** }

But here I got a problem, when I simultaneously query the IMU and and GPS data. There are missing characters inside nmea sentences. For example I got: $GGA,13012.0,,,0,0099.9,,,,*65 instead of $GPGGA,123223.00,,,,,0,00,99.99,,,,,,*65

At the second round I only would like to get the fix GPS data if it available, and store it to SD card.

My questions are: (1) Is this concept good for my aim? (1a) If no: What concept should I have to use? (1b) If yes: How can I get correct nmea sentences? Do I need to simplify the IMU query inside the timer interrupt?

Choose the size of buffer "B" and "A" such a way that filling time always greater than the writing time.

Sorry. This does not make sense. It will ALWAYS be faster to write to an array than to write the array to the SD card for any array size greater than 1.

But here I got a problem, when I simultaneously query the IMU and and GPS data.

Unless you have multiple processors, you can't be simultaneously doing anything.

GPS data arrives on its own schedule. It is up to you to read the data faster than it arrives. You do not query the GPS for data.

Do I need to simplify the IMU query inside the timer interrupt?

I think you need to get rid of the timer interrupt, and use the blink without delay concept, instead.

I really don't understand what you are trying to accomplish. Getting IMU data 256 times per second may, or may not, be possible.

Getting GPS data once per second may, or may not, be possible.

Writing the latest data to the SD buffer at the required rates should be possible.

Expecting to continue getting 256 IMU samples per second while writing the buffer to the card, when the buffer gets full, is NOT realistic.

Domjan: My problem is that, when I set simultaneously turn on the GPS and the IMU in the timer interrupt, there will be missing characters in the NMEA sentences, and I can not get fix GPS data at all.

Are you trying to print too much? Something in your sketch is taking too long and the Serial1 input buffer is overflowing (characters lost). And I'm little surprised that you can do Wire operations inside the TC3_Handler ISR. I'm not familiar with the Due, so are you sure you can do that? Trying to do anything inside an ISR that depends on interrupts (or that takes a long time) is usually bad.

Did you know that you can set up the IMU to generate data at a specified rate and put the samples in its internal FIFO? Then you can read them whenever you get a chance. The compass samples can also go in the FIFO (see section 7.17 of the Product Spec). That will guarantee a constant sampling rate, and decouple the reading rate from the sample rate (i.e., reading the IMU does not have to happen at an exact time).

Like PaulS says,

Expecting to continue getting 256 IMU samples per second while writing the buffer to the card, when the buffer gets full, is NOT realistic.

On one hand, if each IMU sample is 20 bytes, that's 5120 bytes of IMU data per second, plus another ~200 bytes of GPS data. That's not even 6kB/s. The SD card is easily capable of 200kB/s.

On the other hand, waiting to write the data will make the problem worse. When you write to the SD card, it will occasionally block for ~150ms. The IMU FIFO will overflow in ~200ms, and the Serial input buffer will overflow in 67ms. Just write the sample when they're available.

The data rates show that polling all these things in loop will not work; you will have to use an interrupt-driven structure. The primary reason is that SD operations (and maybe the current Serial.prints) will block for too long. GPS data will certainly be lost. If something takes longer that ~200ms, IMU samples will be lost.

If you weren't using a Due, I would suggest looking at the NeoGPS SD logging example (here). It shows how to handle the GPS data during the RX char interrupt. This allows a GPS fix to be assembled while the SD write is blocking. In your case, reading the GPS data in a Timer interrupt may be the only work-around.

There are other reasons to look at NeoGPS, though. It can assemble a single, coherent fix from multiple GPS sentences. The Adafruit library does not guarantee coherency. It doesn't even validate the checksum! NeoGPS is also much faster, but the Due is running at 84MHz, so I don't think that's as important.

Cheers, /dev

Thank your remarks, hints and advice!

Are you trying to print too much?

Reading this link, I am quite sure that is the problem.

I'm little surprised that you can do Wire operations inside the TC3_Handler ISR. I'm not familiar with the Due, so are you sure you can do that?

The linked code without the GPS segment runs without bug for my purposes. My answer that I can, but I agree with that this is not the best and most feasible implementation.

Did you know that you can set up the IMU to generate data at a specified rate and put the samples in its internal FIFO?

No, I didn't. Thanks the hint. I will study this.

On one hand, if .... at ~200ms, IMU samples will be lost. If you weren't using a Due, I would suggest looking at the NeoGPS SD logging example (here).

Thanks these information. Ok I will look, not only just curiosity!

Adam