Weird behavior between Serial3 and SD Card Slot on Teensy 3.6

[tom.rudnick]

Hey,

I'm currently working on programming my own flightcontroller and everything worked fine so far. A few days ago I did the first flying tests and now I wanted to do write some data to a SD Card in order to do a FFT and check if their are any vibrations (Got some oscillations and I want to know if I need to soft mount the gyro).
I use the following Hardware:
- Teensy3.6
- Frsky r-xsr receiver (SBUS) connected to PIN 7
- MPU6050 (i2c) connected to PIN 18, 19
- ESCs are controlled by oneshot125 connected to 20,21,22,23 using FTM0
- SD Card is a SanDisk Ultra micro SD 32GB using the Builtin SD Card module

Now let's come to the problem. When capturing data and writing it to the SD Card sometimes a variable of the calculated GyroValue overflows (not the Raw Gyro value) and that of course leads to a high PID output and lastly to a crash.
Again, this only happens when I capture data. After some hours of debugging I found out if I disable the SBUS receiver and set the receiver values to constant values this error doesn't occur at all.
Here's the piece of code that does reproduce the error:

#include "pid.h"
#include <i2c_t3.h>
#include <SD.h>
#include <SPI.h>

constexpr uint8_t throttle = 0;
constexpr uint8_t pitch = 2;
constexpr uint8_t roll = 1;
constexpr uint8_t yaw = 3;
constexpr uint8_t switch1 = 4;
constexpr uint8_t switch2 = 5;

constexpr uint8_t GyroScale = 0x1B;
constexpr uint8_t MPU_ADDR = 0x68;

constexpr float idle_throttle_percentage = 4.5;

constexpr uint16_t idle_throttle_value = 125.f + 125.f / 100.f * idle_throttle_percentage;


enum QuadState { notArmed, armed, failsafe };

bool blockArming = false;

uint8_t quadStatus = QuadState::notArmed;

float gyro_x_raw, gyro_y_raw, gyro_z_raw; // variables for gyro raw data
float gyro_pitch, gyro_roll, gyro_yaw;
float gyro_x_bias, gyro_y_bias, gyro_z_bias;
constexpr int biasCalculations = 1000;
uint8_t sbus_buffer[25];
uint8_t bufferIndex = 0;
uint16_t sbus_channels[16] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
uint16_t rc_values[8] = { 0,0,0,0,0,0,0,0 };
double setpoint[4] = { 0,0,0,0 };
double rate = 3.2;
double expo = 0.3;

uint8_t esc_1 = 125;
uint8_t esc_2 = 125;
uint8_t esc_3 = 125;
uint8_t esc_4 = 125;

constexpr double pitch_p = 0.05, pitch_i = 0.2, pitch_d = 0.00001;
constexpr double roll_p = 0.05, roll_i = 0.2, roll_d = 0.00001;
constexpr double yaw_p = 0.05, yaw_i = 0.01, yaw_d = 0;

float pitch_pid, roll_pid, yaw_pid;
PID pitch_pid_c = PID();
PID roll_pid_c = PID();
PID yaw_pid_c = PID();

uint32_t timeAfterTakeoff;
bool isQuadLiftedTimer = false;
bool set_I_toZero = false;
bool isQuadLifted = false;
uint32_t takeOffTime = 1000; //in ms

uint32_t nowTime, previousTime;
double dTime;
double deltaTimeS;

float pitch_dError, pitch_errorSum, pitch_error;
float roll_dError, roll_errorSum, roll_error;
float yaw_dError, yaw_errorSum, yaw_error;
float previous_pitch_error, previous_roll_error, previous_yaw_error;
/* MOTOR Konfiguration und Pin Belegung
 *
 * 23      20
 * 4       1
 *  \     /
 *   \ _ /
 *   /   \
 *  /     \
 * 3       2
 * 22      21
 *
 *
 * 1 = esc_1
 * 2 = esc_2
 * 3 = esc_3
 * 4 = esc_4
 *
 *
 */

bool highpitch = false;
long beforeTime, afterTime;

constexpr bool blackboxActive = true;
bool blackboxSDworked = false;

constexpr int blackboxupdaterate = 1000; //in hz
constexpr float blackboxupdatetime = (1.0f / blackboxupdaterate) * pow(10, 6); // 1 / 1000 * 10^6
long blackboxCurrentTime = 0;
long blackboxLastTime = 0;
File blackboxFile;
bool allreadyStopped = false;

void setup() {
  //if(blackboxActive)
  prepareBlackbox();
  
  delay(2000);

  Serial.begin(115200);
  Serial3.begin(100000);
  delay(2000);
  setupMotors();
  
  intializeGyro();
  calcBiasValues();
  calcBiasValues();
  
  //delay(1000);
  
  pinMode(24, OUTPUT);
  pinMode(25, OUTPUT);
  //timeT = millis();

  pitch_pid_c.init(pitch_p, pitch_i, pitch_d);
  roll_pid_c.init(roll_p, roll_i, roll_d);
  yaw_pid_c.init(yaw_p, yaw_i, yaw_d);
  
  nowTime = millis();
  previousTime = nowTime;

}
void loop() {

  if(300000 >= millis())
  {
    readGyroData();
    calcGyroValues();
    readSBUS();
    convertSBusToServoSignal();
    /*rc_values[throttle] = 1000;
    rc_values[pitch] = 1500;
    rc_values[roll] = 1500;
    rc_values[yaw] = 1500;*/
    calculateSetpoint();
    pidController();
    writeMotorValues();
    //Serial.println(setpoint[pitch]);
    writeBlackbox();
  }
  else
  {
    if(!allreadyStopped)
    {
      blackboxFile.close();
      Serial.println("stop");
      allreadyStopped = true;
    }
  }
}

void pidController()
{
  nowTime = micros();
  dTime = nowTime - previousTime;
  previousTime = nowTime;
  deltaTimeS = dTime / 1000000.0;
  

  if (setpoint[throttle] < idle_throttle_value)
  {
    setpoint[throttle] = idle_throttle_value;
  }
  pitch_pid = pitch_pid_c.pidCalc(setpoint[pitch], gyro_pitch, deltaTimeS);
  roll_pid = roll_pid_c.pidCalc(setpoint[roll], gyro_roll, deltaTimeS);
  yaw_pid = yaw_pid_c.pidCalc(setpoint[yaw], gyro_yaw, deltaTimeS);


  uint32_t esc_1_tmp = setpoint[throttle] + pitch_pid + roll_pid - yaw_pid;
  uint32_t esc_2_tmp = setpoint[throttle] - pitch_pid + roll_pid + yaw_pid;
  uint32_t esc_3_tmp = setpoint[throttle] - pitch_pid - roll_pid - yaw_pid;
  uint32_t esc_4_tmp = setpoint[throttle] + pitch_pid - roll_pid + yaw_pid;
  
  
  esc_1 = minMax(esc_1_tmp, idle_throttle_value, 250);
  esc_2 = minMax(esc_2_tmp, idle_throttle_value, 250);
  esc_3 = minMax(esc_3_tmp, idle_throttle_value, 250);
  esc_4 = minMax(esc_4_tmp, idle_throttle_value, 250);


}

void writeMotorValues()
{
  uint16_t esc_1_analog = map(esc_1, 125, 250, 256, 512);
  uint16_t esc_2_analog = map(esc_2, 125, 250, 256, 512);
  uint16_t esc_3_analog = map(esc_3, 125, 250, 256, 512);
  uint16_t esc_4_analog = map(esc_4, 125, 250, 256, 512);

  analogWrite(20, esc_1_analog);
  analogWrite(21, esc_2_analog);
  analogWrite(22, esc_3_analog);
  analogWrite(23, esc_4_analog);
}


void turnOffAllMotors()
{
  esc_1 = 125;
  esc_2 = 125;
  esc_3 = 125;
  esc_4 = 125;
  writeMotorValues();
}

void setupMotors()
{
  analogWriteFrequency(22, 500); //Da nur Pins von FTM0 genutzt werden, ändert sich für die anderen
                   //Pins die Frequenz gleich mit
  analogWriteResolution(12); // 2^12 1us = 2,048 also 125 * 2,048 = 256
  turnOffAllMotors(); //Safety Reasons
}

float convertInputToDegrees(uint16_t input)
{
  double f_factor = (double)1 / (double)20000;
  //todo super rate needs to be added
  double degreeValue = f_factor * rate * (input - 1500) * abs(f_factor * pow((input - 1500), 3)) + expo * (input - 1500);
  return degreeValue;
}

void calculateSetpoint()
{
  setpoint[throttle] = map(rc_values[throttle], 1000, 2000, 125, 250);
  setpoint[pitch] = convertInputToDegrees(rc_values[pitch]);
  setpoint[roll] = convertInputToDegrees(rc_values[roll]);
  setpoint[yaw] = convertInputToDegrees(rc_values[yaw]);
}


void convertSBusToServoSignal()
{
  rc_values[throttle] = map(sbus_channels[throttle], 172, 1811, 1000, 2000);
  rc_values[pitch] = map(sbus_channels[pitch], 172, 1811, 1000, 2000);
  rc_values[roll] = map(sbus_channels[roll], 172, 1811, 1000, 2000);
  rc_values[yaw] = map(sbus_channels[yaw], 172, 1811, 1000, 2000);
  rc_values[switch1] = map(sbus_channels[switch1], 172, 1811, 1000, 2000);
  rc_values[switch2] = map(sbus_channels[switch2], 172, 1811, 1000, 2000);

}



void readGyroData()
{
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x43); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR, 3 * 2, true); // request a total of 7*2=14 registers

  int16_t gyro_x_raw_i = Wire.read() << 8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
  int16_t gyro_y_raw_i = Wire.read() << 8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
  int16_t gyro_z_raw_i = Wire.read() << 8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)
  gyro_x_raw = float(gyro_x_raw_i);
  gyro_y_raw = float(gyro_y_raw_i);
  gyro_z_raw = float(gyro_z_raw_i);
}


void readSBUS()
{
  if (Serial3.available() >= 25)
  {
    bufferIndex = 0;
    while (Serial3.available() > 0) {
      uint8_t data = Serial3.read();
      sbus_buffer[bufferIndex] = data;
      bufferIndex++;
    }

    if (bufferIndex == 25)
    {
      if (sbus_buffer[0] == 0x0f && sbus_buffer[24] == 0x00)
      {
        sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
        sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
        sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
        sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
        sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
        sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);

      }
    }
  }
}



void calcGyroValues()
{
  gyro_pitch = -1.0 * ((gyro_x_raw - gyro_x_bias) / 16.4);
  gyro_roll = -1.0 * ((gyro_y_raw - gyro_y_bias) / 16.4);
  gyro_yaw = (gyro_z_raw - gyro_z_bias) / 16.4;

}

void calcBiasValues()
{
  int32_t gyro_x_tmp = 0;
  int32_t gyro_y_tmp = 0;
  int32_t gyro_z_tmp = 0;
  for (int i = 0; i < biasCalculations; i++)
  {
    readGyroData();
    gyro_x_tmp += gyro_x_raw;
    gyro_y_tmp += gyro_y_raw;
    gyro_z_tmp += gyro_z_raw;
  }
  gyro_x_bias = gyro_x_tmp / biasCalculations;
  gyro_y_bias = gyro_y_tmp / biasCalculations;
  gyro_z_bias = gyro_z_tmp / biasCalculations;
}

void intializeGyro()
{
  Wire.begin(I2C_MASTER, MPU_ADDR, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0x01);
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(GyroScale);
  Wire.write(0x18);
  Wire.endTransmission(true);
  /*Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1A); // Digital Low Pass Filter
  Wire.write(6);
  Wire.endTransmission(true);*/
}


float minMax(float value, float min, float max)
{
  if (value < min) return min;
  if (value > max) return max;
  return value;
}

bool prepareBlackbox()
{
  if(!SD.begin(BUILTIN_SDCARD))
    return false;
  blackboxFile = SD.open("log.txt", FILE_WRITE);
  if(blackboxFile)
  {
    blackboxFile.println("deltaTime;Gpr;Grr;Gyr;Gp;Gr;Gy;esc1;esc2;esc3;esc4;pidp;pidr;pidy;throttle;pitch;roll;yaw");
    return true;
  }
  else
    return false;
  blackboxCurrentTime = 0;
}


void writeBlackbox()
{
    
    blackboxCurrentTime = micros();
    blackboxFile.print(blackboxCurrentTime - blackboxLastTime); blackboxFile.print(";");
    Serial.println(blackboxCurrentTime-blackboxLastTime);
    blackboxFile.print(gyro_x_raw); blackboxFile.print(";");
    blackboxFile.print(gyro_y_raw); blackboxFile.print(";");
    blackboxFile.print(gyro_z_raw); blackboxFile.print(";");
    blackboxFile.print(gyro_pitch); blackboxFile.print(";");
    blackboxFile.print(gyro_roll); blackboxFile.print(";");
    blackboxFile.print(gyro_yaw); blackboxFile.print(";");
    blackboxFile.print(esc_1); blackboxFile.print(";");
    blackboxFile.print(esc_2); blackboxFile.print(";");
    blackboxFile.print(esc_3); blackboxFile.print(";");
    blackboxFile.print(esc_4); blackboxFile.print(";");
    blackboxFile.print(pitch_pid); blackboxFile.print(";");
    blackboxFile.print(roll_pid); blackboxFile.print(";");
    blackboxFile.print(yaw_pid); blackboxFile.print(";");
    blackboxFile.print(setpoint[throttle]); blackboxFile.print(";");
    blackboxFile.print(setpoint[pitch]); blackboxFile.print(";");
    blackboxFile.print(setpoint[roll]); blackboxFile.print(";");
    blackboxFile.print(setpoint[yaw]); blackboxFile.println(";");
    blackboxFile.flush();  
    blackboxLastTime = blackboxCurrentTime;
}

I'm 99.9% PID.h is not the Problem but maybe I miss something so here it is:

// pid.h

#ifndef _PID_h
#define _PID_h

class PID {
public:
  PID();
  PID(float _kp, float _ki, float _kd);
  void init(float _kp, float _ki, float _kd);
  float pidCalc(float setpoint, float position, double dt);
  void setKP(float _kp);
  void setKI(float _ki);
  void setKD(float _kd);
  void resetErrorSum();
  float getErrorSum();

private:
  float dError, errorSum, error;
  float previousError = 0;
  float kp, ki, kd;

};
#endif

PID.CPP

// 
// 
// 

#include "pid.h"

PID::PID()
{

}


PID::PID(float _kp, float _ki, float _kd)
{
  kp = _kp;
  ki = _ki;
  kd = _kd;
}

void PID::init(float _kp, float _ki, float _kd)
{
  kp = _kp;
  ki = _ki;
  kd = _kd;
}

float PID::pidCalc(float setpoint, float position, double dt)
{
  error = (int)position - (int)setpoint;
  errorSum += error * dt;
  dError = kd * (error - previousError) / dt;
  previousError = error;

  //
  return kp * error + ki * errorSum + dError;
}

void PID::setKP(float _kp)
{
  kp = _kp;
}

void PID::setKI(float _ki)
{
  ki = _ki;
}

void PID::setKD(float _kd)
{
  kd = _kd;
}


void PID::resetErrorSum()
{
  errorSum = 0;
}

float PID::getErrorSum()
{
  return errorSum;
}

The loop of the flightcontroller is of course changed in order to capture 5 mins of data. A piece of data looks like this:

If you now comment out the readSBUS() function everything works as expected and you don't get any overflows.
Does anybody know why this happens and how to resolve this issue? Or am I just dump and did a mistake?
Thanks in advance, feel free to correct my bad English

 

[Wibbing]

Before removing the error i would change my program , so that this error will not result in a crash.
This is quite easy in this case: if you find an Overflow replace it by the last good result.
So you will have a little "ruckel" but no crash.

After this you can correct the error.

 

[defragster]

Serial3 on T_3.6 only has a single byte cpu FIFO - it needs an interrupt to move data to Rx buffer.

Any chance you might use Serial 1 or 2 for Rx? They have 8 byte FIFO.

Found a page for Frsky r-xsr receiver - but it doesn't say much about the device - how much data it might send ? Assuming it is the incoming data from the remote control - and that that could get to be busy/continuous? This isn't super fast at : Serial3.begin(100,000), but at 100 baud per ms a delay of 1 ms misses 10 bytes of data?

Dealing with SD can cause big lags if the card stops for internal cleanup or file system overhead. Not sure if the Serial3 interrupt is higher priority - or maybe a long delay is overflowing the default input buffer even if the bits aren't lost before buffering? At 10 bytes/sec - not reading data for 6.4 ms leaves no room for the next byte.

If you can do not destructive repro a quick change to this "#define SERIAL3_RX_BUFFER_SIZE 64 " here ...\hardware\teensy\avr\cores\teensy3\serial3.c might show if buffer is over flowing - code could add tracking of Serial3.available() counts to see if the buffer is ever growing in size toward that 64.

And seeing:

void readSBUS()
{
  if (Serial3.available() >= 25)

Means the system buffers data longer than needed leaving it partly full between checks.

And this "if(300,000 >= millis())" is a long time between checking that, if 3 messages arrive in that time there will be data lost.

If there is data pulling it to a larger buffer { above RX_BUFFER or a local sketch buffer for later parsing } sooner wouldn't take much time and prevent data loss.

 

[tom.rudnick]

Thanks for the fast answer.
Unfortunately Serial1 and Serial2 both don't work. I can't explain why but the code only works at Serial3.
The r-xsr uses the SBUS Protocol which always uses the Baudrate of 100,000. You might find out more here: https://github.com/uzh-rpg/rpg_quadrotor_control/wiki/SBUS-Protocol
For the Arduino and the ESP this library exists: https://github.com/bolderflight/SBUS.

When it comes to the "if(300,000 >= millis())", I don't loose data every 3 messages. I provide the full csv file in the attachments: View attachment logExcel.zip (Search for ovf in order to see where the data is corrupt).

I'm not sure what you mean by changing: "#define SERIAL3_RX_BUFFER_SIZE 64" because it's already like this in the serial3.c file. But if I understand you correct I should print Serial3.available() to the SD Card in order to see if it is raising above 64. If I'm wrong please provide some further instructions.
This Data loss which you mentioned is indeed a big problem but I don't know which other options I have. I would store the data in an Array and write it later to the SD Card but I'm pretty sure there's not enough storage for that on the teensy. Or is it maybe an option to buffer the data in an 512Byte Array in order to get the best performance?
If this issue could get resolved by switching to the Teensy 4.0 this would be a suitable option since I need a smaller form factor later anyway. (SD Slot could I put on top)

 

[tom.rudnick]

Before removing the error i would change my program , so that this error will not result in a crash.
This is quite easy in this case: if you find an Overflow replace it by the last good result.
So you will have a little "ruckel" but no crash.

After this you can correct the error.

Already done, I don't even connect the battery so nothing could happen but I'm not able to to resolve this issue.

 

[KurtE]

I agree with with the suggestions that @defragster mentioned.

Not sure why Serial1 and Serial2 would not work, but back to the issue at hand....

uint8_t sbus_buffer[25];
uint8_t bufferIndex = 0;
...
void readSBUS()
{
  if (Serial3.available() >= 25)
  {
    bufferIndex = 0;
    [COLOR="#FF0000"]while (Serial3.available() > 0) {
      uint8_t data = Serial3.read();
      sbus_buffer[bufferIndex] = data;
      bufferIndex++;
    }[/COLOR]

    if (bufferIndex == 25)
    {
      if (sbus_buffer[0] == 0x0f && sbus_buffer[24] == 0x00)
      {
        sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
        sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
        sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
        sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
        sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
        sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);

      }
    }
  }
}

Suppose the SD Card adds in a delay and during that time more data came in, like maybe the beginning of the next message and you have 30 bytes in the Serial3 queue.

Your code I marked in RED will now corrupt memory by overwriting the 5 bytes after the sbus_buffer, probably including the buffer index variable...

 

[tom.rudnick]

I agree with with the suggestions that @defragster mentioned.

Not sure why Serial1 and Serial2 would not work, but back to the issue at hand....

uint8_t sbus_buffer[25];
uint8_t bufferIndex = 0;
...
void readSBUS()
{
  if (Serial3.available() >= 25)
  {
    bufferIndex = 0;
    [COLOR="#FF0000"]while (Serial3.available() > 0) {
      uint8_t data = Serial3.read();
      sbus_buffer[bufferIndex] = data;
      bufferIndex++;
    }[/COLOR]

    if (bufferIndex == 25)
    {
      if (sbus_buffer[0] == 0x0f && sbus_buffer[24] == 0x00)
      {
        sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
        sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
        sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
        sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
        sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
        sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);

      }
    }
  }
}

Suppose the SD Card adds in a delay and during that time more data came in, like maybe the beginning of the next message and you have 30 bytes in the Serial3 queue.

Your code I marked in RED will now corrupt memory by overwriting the 5 bytes after the sbus_buffer, probably including the buffer index variable...

Ok I guess I understand the problem now, I'm still not sure why this causes an overflow at the gyro variables but still this needs to be fixed. So would it be possible to use Serial3 with some kind of an interrupt? Or is it possible to do the logging more efficient without this huge delay the SD card causes. According to the deltaTime in the log the average seems to be 4000 Microseconds which is way to long. Have you any ideas how to log data efficiently ? I need at least 1000khz log file rate.
Thanks in advance

 

[KurtE]

Again I have not looked at your data format or the like, so I will only go here by generalizations.

I am not really sure how your main loop is supposed to work:

void loop() {

  if(300000 >= millis())
  {
    readGyroData();
    calcGyroValues();
    readSBUS();
    convertSBusToServoSignal();
    /*rc_values[throttle] = 1000;
    rc_values[pitch] = 1500;
    rc_values[roll] = 1500;
    rc_values[yaw] = 1500;*/
    calculateSetpoint();
    pidController();
    writeMotorValues();
    //Serial.println(setpoint[pitch]);
    writeBlackbox();
  }
  else
  {
    if(!allreadyStopped)
    {
      blackboxFile.close();
      Serial.println("stop");
      allreadyStopped = true;
    }
  }
}

But I read it as if the program has run less than 30 seconds, and this is the first call to loop, than close the file blackBoxFile.

If it has run >= 30 seconds, than call off to all of your other stuff, every time... So once 30 seconds have elapsed it will do all of the main stuff as fast as it can...

So if it were me, I would probably rework the readSBUS code to process as much of the data as possible... Maybe also adding in some other validations while I was at it.

void readSBUS()
{
  if (Serial3.available() > 25) 
  {
    while (Serial3.read() != -1) ; // to much data toss it all.
  }
  else if (Serial3.available() == 25)
  {
    for(bufferIndex = 0; bufferIndex < 25; bufferIndex++) 
    {
      uint8_t data = Serial3.read();
      sbus_buffer[bufferIndex] = data;
    }

    if (sbus_buffer[0] == 0x0f && sbus_buffer[24] == 0x00)
    {
      sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
      sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
      sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
      sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
      sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
      sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);
    }
  }
}

The above works sort of like your current one, but does not overwrite other memory. It will loose data, in a similar way to you already do
That is suppose your data is such that you received 24 bytes before the iteration of loop is called. So this code does not run. But right after this the 25th byte arrives, but during the processing enough time has elapsed such that it starts to receive the next message, lets say the first 3 bytes came in... So your code comes along and properly processes the first message, you just tossed those three bytes of the next packet. Your code will then come around again and receives the rest of the next message. So it got those 22 bytes, your code does not think there is enough data so again it won't process them until the start of the next message... Where you get the first N bytes of the next next message... Which may look valid may not... But rest of buffer tossed... Hopefully at some point you get enough of a delay that you can get a full buffer to be processed again...

But again this looses data! Another approach would be to have the readSBUS keep some state information and process whatever bytes it has received, and try to keep in sync...
Some psuedo code that may be made to work...

uint8_t sbus_buffer[25];
uint8_t bufferIndex = 0;
elapsedMillis bufferTimer = 0;
#define   BUFFER_TIMEOUT 5  // No idea what value this should be for your data?

void readSBUS()
{
  if (Serial3.available()) 
  {
    if (bufferTimer > BUFFER_TIMEOUT) bufferIndex = 0;  // too long reset to start...
    uint8_t data;

    while ((data = Serial3.read()) != -1)
    {
        if (dataIndex == 0)
        {
            if (data == 0x0f) 
            {
                 // only get out of this state if valid start... 
                 sbus_buffer[bufferIndex++] = data;
            }
        }
        else 
        {
             sbus_buffer[bufferIndex++] = data;
        }
        if (dataIndex == 25)
        {
            if (sbus_buffer[24] == 0x00)
            {
                sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
                sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
                sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
                sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
                sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
                sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);
                bufferIndex = 0
                break; 
            }
            else 
            {
                // data was not valid... how to recover... 
                // Could simply punt, reset buffer Index to 0
                // Or could maybe see if we have start of other possible valid packet... 
                // maybe something like:
                uint8_t i;
                for (i = 0; i < 25; i++) {
                    if (sbus_buffer[i] == 0xf) 
                    {
                        // need to set new buffer index copy rest of data down... 
                        bufferIndex = 25 - i; // double check 
                        for (uint8_t j = 0; j < bufferIndex; j++) sbus_buffer[j] = sbus_buffer[j + i]; 
                        break;
                    } 
                }

            }
        }
    }
    bufferTimer = 0; // reset our timer after processing data
  }
}

But again there are probably lots of bugs in the above

 

[mjs513]

@brtaylor has a SBUS library that works on the Teensy's: https://github.com/bolderflight/SBUS. Suggest you check it out it may help. It looks like it has some checks on sbus data that you can use:

bool read(uint16_t* channels, bool* failsafe, bool* lostFrame) read(uint16_t* channels, bool* failsafe, bool* lostFrame) reads data from the SBUS receiver and parses the SBUS packet. When a complete packet is received, read(uint16_t* channels, bool* failsafe, bool* lostFrame) returns true and the channels[0-15], failsafe, and lost frame data is available. Note that lost frame is true if the frame was lost and failsafe is true if the receiver has entered failsafe mode. Lost frame is typically used as part of a counter to collect lost frames data for evaluating receiver performance. For example, placing the following code in the loop function will print the value of channel 0 every time a valid SBUS packet is received.

 

[tom.rudnick]

Thanks again for the detailed answer. I will try to implement this later or tomorrow and share the result here.
One more question, how fast can the SD Module write the data? Is it still possible to have a refresh rate above 1kHz?

 

[KurtE]

I don't do a lot of SD stuff, so not sure of the complete answer. Hopefully others can give you a more complete answer.

When I do play with SD, I often go over to use libraries like SDFat (or the beta version).
https://github.com/greiman/SdFat-beta

Some of the things to consider/try, is most devices like this work best when your writes are some size or integral count of that size. Again it has been awhile, since I played as much with this.
But it is usually something like the sector size or cluster size... Which is probably something like 256 or 512 bytes...

With some systems, you can improve the speed, is to try to start off with an SDCard which is not fragments, if it is, either Defrag it or reformat it... Then when you start up the logging try to pre-allocate storage to your log file. That is if you know your log file is likely to be something like n Megabytes, try to have the system allocate the space before you start your logging. Again it has been awhile since I tried this, but on some systems you can do this, by Opening the file, then Seek to the size that you think the file will be, and then Seek back to the beginning.

But if your code does not cleanly close out the file, your log file may show lots of garbage starting at where your code left off. I know there are tricks on to take care of it. In clean close down, your code might tell the file system to truncate the file at the current write point and then close.

But again it has been awhile.

It probably goes without saying, but if your code is setup, that a logical writing of a log record works like:
Open File, Seek to end, write new data, close file ...

It will be a lot slower than keeping the file open...

 

[defragster]

Not done much on SD either - KurtE's notes are on point AFAIK - the sector size is 512 Bytes - that is buffered in the code and fits cleanly on SD card, other than that the SD card may do lots of write/read/rewrite in worst case.

Search for Low Latency Logger here on Forum - maybe with Bing or the other gOO thing.

Teensy 3.6 native SDIO can write perhaps at 10 MB /sec I've seen with the greiman/SdFat-beta - also a thread

 

[tom.rudnick]

I think I finally managed the SBUS Function to work together with the SD Card.

Here's the new readSbus() Function:

void readSBUS()
{
  if (Serial3.available())
  {
    uint8_t data;

    while (Serial3.available() > 0)
    {
      data = Serial3.read();
      if(bufferIndex == 25)
      {
        if(sbus_buffer[24] == 0x00 && sbus_buffer[0] == 0x0f)
        {
          sbus_channels[0] = ((sbus_buffer[1] | sbus_buffer[2] << 8) & 0x07FF);
          sbus_channels[1] = ((sbus_buffer[2] >> 3 | sbus_buffer[3] << 5) & 0x07FF);
          sbus_channels[2] = ((sbus_buffer[3] >> 6 | sbus_buffer[4] << 2 | sbus_buffer[5] << 10) & 0x07FF);
          sbus_channels[3] = ((sbus_buffer[5] >> 1 | sbus_buffer[6] << 7) & 0x07FF);
          sbus_channels[4] = ((sbus_buffer[6] >> 4 | sbus_buffer[7] << 4) & 0x07FF);
          sbus_channels[5] = ((sbus_buffer[7] >> 7 | sbus_buffer[8] << 1 | sbus_buffer[9] << 9) & 0x07FF);
          bufferIndex = 0;
        }
        else
        {
           bufferIndex = 0;
        }
      } 
      if(bufferIndex == 0)
      {
        if(data == 0x0f)
          sbus_buffer[bufferIndex++] = data;
      }
      else {         
         sbus_buffer[bufferIndex++] = data;
      }

    }
  }
}

According to the log I've got a speed between 2.1 kHz and 3.4 kHz which is pretty decent for the moment. Maybe I can speed it up if I optimize the SD card code like you guys told me.
Again, thank you very much guys.