Teensy 3.6 SPI and SD interference

Status
Not open for further replies.
M

MarkXal

New member
Hello everyone,

I have an interesting problem which i suspect is somewhat common, but I have not been able to find a solution to. I am working on some code for an IMU that used an ADIS16364 sensor wired to a Teensy 3.6 using SPI. I want to do my IMU calculations then log the results on the built-in SD card. If I just calculate the values and output them to the USB serial, everything works, but as soon as I add the SD code the entire thing breaks. The sensor SPI start reading erratically, the serial data is all NaN, and it actually gets hard to even reverse the "damage", as multiple uploads of the program without SD references did not fix the problem. After a few board restarts I was able to get the code working again without SD.
My hypothesis is that the SD library does some background reconfiguration to the SPI logic that interferes with all SPI channels and not just the dedicated SD one.
Code below:

Code: 
#include <SD.h>
#include <SPI.h>

int16_t SPI_Data=0x0000;
double gyro[3]={0,0,0};
double acc[3]={0,0,0};
double acc_new[3]={0,0,0};
double R[3][3];
float alpha = 0.1;
const float g = 1;
float alpha_gain=0;
float acc_magn=0;
long sum[3]={0,0,0};
double Rot_Quaternion[4] = {1, 0, 0, 0};
double Rot_Quaternion_new[4] = {1, 0, 0, 0};
double Grav_Quaternion[4] = {1, 0, 0, 0};
double Adj_Quaternion[4] = {1, 0, 0, 0};
float pitch=0;
float yaw=0;
float roll=0;

float norm;
float norm2;
float deltaT;

elapsedMicros calculateNow;
elapsedMillis sendMessage;

void setup() {

  pinMode(10, OUTPUT);
  Serial.begin(9600);
  digitalWrite(10, HIGH);
  SPI.begin();
  SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));

  digitalWrite(10, LOW);
  delayMicroseconds(10);
  SPI_Data = (SPI.transfer16(0x0400) << 2);
  delayMicroseconds(10);
  digitalWrite(10, HIGH);
  delay(1);

  SD.begin(BUILTIN_SDCARD);
  File logFile = SD.open("log.txt",FILE_WRITE);
  logFile.println("pitch,roll,yaw");
  logFile.close();
  
}

void loop() {

  if (calculateNow > 10000 ) {
    deltaT = calculateNow;
    delayMicroseconds(50);
    digitalWrite(10, LOW);
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0600) << 2);
    SPI_Data = SPI_Data / 4 ;
    gyro[0] = (float)SPI_Data * 0.05;
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0800) << 2);
    SPI_Data = SPI_Data / 4 ;
    gyro[1] = (float)SPI_Data * 0.05;
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0A00) << 2);
    SPI_Data = SPI_Data / 4 ;
    gyro[2] = (float)SPI_Data * 0.05;
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0C00) << 2);
    SPI_Data = SPI_Data / 4;
    acc[0] = (float)SPI_Data * 0.001;
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0E00) << 2);
    SPI_Data = SPI_Data / 4;
    acc[1] = (float)SPI_Data * 0.001;
    delayMicroseconds(50);
    SPI_Data = (SPI.transfer16(0x0400) << 2);
    SPI_Data = SPI_Data / 4;
    acc[2] = -(float)SPI_Data * 0.001;
    digitalWrite(10, HIGH);

    Rot_Quaternion[0] += (-Rot_Quaternion[1] * gyro[0] - Rot_Quaternion[2] * gyro[1] - Rot_Quaternion[3] * gyro[2]) / (deltaT);
    Rot_Quaternion[1] += ( Rot_Quaternion[0] * gyro[0] + Rot_Quaternion[2] * gyro[2] - Rot_Quaternion[3] * gyro[1]) / (deltaT) ;
    Rot_Quaternion[2] += ( Rot_Quaternion[0] * gyro[1] - Rot_Quaternion[1] * gyro[2] + Rot_Quaternion[3] * gyro[0]) / (deltaT);
    Rot_Quaternion[3] += ( Rot_Quaternion[0] * gyro[2] + Rot_Quaternion[1] * gyro[1] - Rot_Quaternion[2] * gyro[0]) / (deltaT);

    norm = sqrt(Rot_Quaternion[0] * Rot_Quaternion[0] + Rot_Quaternion[1] * Rot_Quaternion[1] + Rot_Quaternion[2] * Rot_Quaternion[2] + Rot_Quaternion[3] * Rot_Quaternion[3]);

    Rot_Quaternion[0] /= norm;
    Rot_Quaternion[1] /= norm;
    Rot_Quaternion[2] /= norm;
    Rot_Quaternion[3] /= norm;

    if (Rot_Quaternion[0] < 0)
    {
      Rot_Quaternion[1] = -Rot_Quaternion[1];
      Rot_Quaternion[2] = -Rot_Quaternion[2];
      Rot_Quaternion[3] = -Rot_Quaternion[3];
    }

    R[0][0]=Rot_Quaternion[0]*Rot_Quaternion[0]+Rot_Quaternion[1]*Rot_Quaternion[1]-Rot_Quaternion[2]*Rot_Quaternion[2]-Rot_Quaternion[3]*Rot_Quaternion[3];
    R[1][0]=2*(Rot_Quaternion[1]*Rot_Quaternion[2]+Rot_Quaternion[0]*Rot_Quaternion[3]);
    R[2][0]=2*(Rot_Quaternion[1]*Rot_Quaternion[3]-Rot_Quaternion[0]*Rot_Quaternion[2]);
    R[0][1]=2*(Rot_Quaternion[1]*Rot_Quaternion[2]-Rot_Quaternion[0]*Rot_Quaternion[3]);
    R[1][1]=Rot_Quaternion[0]*Rot_Quaternion[0]-Rot_Quaternion[1]*Rot_Quaternion[1]+Rot_Quaternion[2]*Rot_Quaternion[2]-Rot_Quaternion[3]*Rot_Quaternion[3];
    R[2][1]=2*(Rot_Quaternion[2]*Rot_Quaternion[3]+Rot_Quaternion[0]*Rot_Quaternion[1]);
    R[0][2]=2*(Rot_Quaternion[1]*Rot_Quaternion[3]+Rot_Quaternion[0]*Rot_Quaternion[2]);
    R[1][2]=2*(Rot_Quaternion[2]*Rot_Quaternion[3]-Rot_Quaternion[0]*Rot_Quaternion[1]);
    R[2][2]=Rot_Quaternion[0]*Rot_Quaternion[0]-Rot_Quaternion[1]*Rot_Quaternion[1]-Rot_Quaternion[2]*Rot_Quaternion[2]+Rot_Quaternion[3]*Rot_Quaternion[3];

    acc_new[0]=R[0][0]*acc[0]+R[1][0]*acc[1]+R[2][0]*acc[2];
    acc_new[1]=R[0][1]*acc[0]+R[1][1]*acc[1]+R[2][1]*acc[2];
    acc_new[2]=R[0][2]*acc[0]+R[1][2]*acc[1]+R[2][2]*acc[2];

    acc[0]=acc_new[0];
    acc[1]=acc_new[1];
    acc[2]=acc_new[2];

    acc_magn = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
    acc[0] /= acc_magn;
    acc[1] /= acc_magn;
    acc[2] /= acc_magn;


    if (acc[2]>=0){
    Grav_Quaternion[0] = sqrt((acc[2] + 1) / 2);
    Grav_Quaternion[1] = -acc[1] / sqrt(2 * (acc[2] + 1));
    Grav_Quaternion[2] = acc[0] / sqrt(2 * (acc[2] + 1));
    Grav_Quaternion[3] = 0;
    }
    else if (acc[2]<0){
    Grav_Quaternion[0] = -acc[1]/sqrt(1-acc[2]);
    Grav_Quaternion[1] = sqrt((1-acc[2])/2);
    Grav_Quaternion[2] = 0;
    Grav_Quaternion[3] = acc[0]/sqrt(2*(1-acc[2]));     
    }
    
    alpha_gain = abs(acc_magn - g) / g;

    if (alpha_gain <= 0.1) {
      alpha_gain = 1;
    }
    else if (alpha_gain<0.2 and alpha_gain>0.1) {
      alpha_gain = (0.1 - alpha_gain) * 10 + 1;
    }
    else {
      alpha_gain = 0;
    }

    Adj_Quaternion[0] = (1 - alpha * alpha_gain) + (alpha * alpha_gain) * Grav_Quaternion[0];
    Adj_Quaternion[1] = (alpha * alpha_gain) * Grav_Quaternion[1];
    Adj_Quaternion[2] = (alpha * alpha_gain) * Grav_Quaternion[2];
    Adj_Quaternion[3] = 0;

    norm2 = sqrt(Adj_Quaternion[0] * Adj_Quaternion[0] + Adj_Quaternion[1] * Adj_Quaternion[1] + Adj_Quaternion[2] * Adj_Quaternion[2] + Adj_Quaternion[3] * Adj_Quaternion[3]);

    Adj_Quaternion[0] /= norm2;
    Adj_Quaternion[1] /= norm2;
    Adj_Quaternion[2] /= norm2;
    Adj_Quaternion[3] /= norm2;

    Rot_Quaternion_new[0] = Rot_Quaternion[0] * Adj_Quaternion[0] - Rot_Quaternion[1] * Adj_Quaternion[1] - Rot_Quaternion[2] * Adj_Quaternion[2] - Rot_Quaternion[3] * Adj_Quaternion[3];
    Rot_Quaternion_new[1] = Rot_Quaternion[0] * Adj_Quaternion[1] + Rot_Quaternion[1] * Adj_Quaternion[0] + Rot_Quaternion[2] * Adj_Quaternion[3] - Rot_Quaternion[3] * Adj_Quaternion[2];
    Rot_Quaternion_new[2] = Rot_Quaternion[0] * Adj_Quaternion[2] - Rot_Quaternion[1] * Adj_Quaternion[3] + Rot_Quaternion[2] * Adj_Quaternion[0] + Rot_Quaternion[3] * Adj_Quaternion[1];
    Rot_Quaternion_new[3] = Rot_Quaternion[0] * Adj_Quaternion[3] + Rot_Quaternion[1] * Adj_Quaternion[2] - Rot_Quaternion[2] * Adj_Quaternion[1] + Rot_Quaternion[3] * Adj_Quaternion[0];

    Rot_Quaternion[0] = Rot_Quaternion_new[0];
    Rot_Quaternion[1] = Rot_Quaternion_new[1];
    Rot_Quaternion[2] = Rot_Quaternion_new[2];
    Rot_Quaternion[3] = Rot_Quaternion_new[3];
    
    calculateNow = 0;
  }

  if (sendMessage > 100 ) {

    sendMessage = 0;

    pitch = Rot_Quaternion[1];
    roll =  Rot_Quaternion[2];
    yaw =  Rot_Quaternion[3];
    
    Serial.print(pitch);
    Serial.print(",");
    Serial.print(roll);
    Serial.print(",");
    Serial.println(yaw);

    File logFile = SD.open("log.txt",FILE_WRITE);
    logFile.print(pitch);
    logFile.print(",");
    logFile.print(roll);
    logFile.print(",");
    logFile.println(yaw);
    logFile.close();
  }
}
 
My first advice would be: Don't open and close your file every time you write to it. Opening and closing can consume a lot of CPU cycles and are not really necessary in the loop().

Instead, open the file in your setup() function. Then call logFile.sync() after each write. The open() / close() pair can take over 100mSec when the SD card has to erase a new data block.

When you expand your example code to a real collection program, you will have some way to stop the data collection. At that point, you can close the data file.

Also, it's late in the evening here but I think"SD.open("log.txt",FILE_WRITE)" appends at the end of the file---in which case the file write has to traverse the FAT table at the open() call to find the end of the file. That's going to take one or more extra block reads to get to the end of the file.

When you get comfortable with the simple SD file stuff, you should investigate SDFat beta 2.0. It allows you to implement an EXFat file system, which is like manna from heaven for those who want to implement efficient data logging.

Take heart: I know from experience that it is possible to log data from SPI devices at up to about 800KBytes/second when you get all the bugs ironed out. However to get this kind of logging speed, you're going to have to look into storing your data in binary format.


/ rant on

Is it just me, or do others detect a prejudice against the use of Serial.printf() or sprintf()? Yes, these functions do add a few hundreds or thousands of bytes of code---but who is running out of code space on a T3.6???? Yes, these functions are not usually thread-safe, but that isn't an issue for single-threaded Arduino programs.

Code: 
    Serial.print(pitch);
    Serial.print(",");
    Serial.print(roll);
    Serial.print(",");
    Serial.println(yaw);

    File logFile = SD.open("log.txt",FILE_WRITE);
    logFile.print(pitch);
    logFile.print(",");
    logFile.print(roll);
    logFile.print(",");
    logFile.println(yaw);
    logFile.close();

   becomes a lot more compact with:

    char outstring[100];  // 100 may be overkill, but better safe than sorry
    sprintf(outstring,"%8.3f, %8.3f, %8.3f\n", pitch, roll, yaw);
    Serial.print(outstring);
    logFile.print(outstring);
    logFile.sync();   // Hmmm---is sync() available in the standard SD lib, or do you need SDFat???

/rant off
 
mborgerson said:
/ rant on

Is it just me, or do others detect a prejudice against the use of Serial.printf() or sprintf()? Yes, these functions do add a few hundreds or thousands of bytes of code---but who is running out of code space on a T3.6???? Yes, these functions are not usually thread-safe, but that isn't an issue for single-threaded Arduino programs.


/rant off
Click to expand...


Sorry! I don't know much C++ so using printf never came to mind! It's good to hear that you know it's possible to datalog SPI to SD, so clearly some timing issue as you have been describing must be hanging up the Teensy.
 
On Rant ;)

For my own stuff I like using printf, but if I know that same code is going to run on several platforms, like some of the actual Arduino ones, they may not support it...
Wish they did!
 
printf() and sprintf() are not C++ functions. They are documented in my original K&R "The C Programming Language" from 1988. As long as you have the C stdio library, you can use the

sprintf(str, ...}
Serial.print(str);

combination to reduce many lines of Serial.print() calls down to just two lines. This should work even on non-Teensy Arduino systems that don't implement Serial.printf().

It's important to remember that these functions, including Serial.print(), don't belong in interrupt handlers. That limitation can make debugging interrupt handlers more difficult as you need to store any debug information in a place where the main loop can fetch it for display.
 
Status
Not open for further replies.
Back
Top