Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 10 of 10

Thread: Incredibly slow loop time on Teensy 4.0

  1. #1
    Junior Member
    Join Date
    Aug 2022
    Posts
    11

    Incredibly slow loop time on Teensy 4.0

    Hello,

    I'm surprised by the loop cycle time I get on Teensy 4.0.

    The loop is the following one:

    Code:
    void loop() {
      int servoValue = 0;
    
         cycle = !cycle;
        digitalWrite(CYCLE_TIME, cycle);
    
    }
    So `CYCLE_TIME` is inverted each loop which gives a cycle time 4.5 ms where I get less than 1.5 ms on an ESP32 board running at 'only' 160 MHz, and where this `loop()` function is in fact a task in FreeRTOS, reading an ICM_20948 and driving a servo motor!!!

    I guess there is maybe an initialisation problem, I do not do anything special in `setup()` except setting the ICM_20948.*

    Where could the problem come from??

  2. #2
    Senior Member
    Join Date
    Dec 2013
    Location
    East Stroudsburg PA.
    Posts
    320
    try digitalWriteFast(CYCLE_TIME, cycle);

  3. #3
    Senior Member
    Join Date
    Apr 2020
    Location
    DFW area in Texas
    Posts
    511
    Quote Originally Posted by fralbo View Post
    Hello,

    I'm surprised by the loop cycle time I get on Teensy 4.0.

    The loop is the following one:

    Code:
    void loop() {
      int servoValue = 0;
    
         cycle = !cycle;
        digitalWrite(CYCLE_TIME, cycle);
    
    }
    So `CYCLE_TIME` is inverted each loop which gives a cycle time 4.5 ms where I get less than 1.5 ms on an ESP32 board running at 'only' 160 MHz, and where this `loop()` function is in fact a task in FreeRTOS, reading an ICM_20948 and driving a servo motor!!!

    I guess there is maybe an initialisation problem, I do not do anything special in `setup()` except setting the ICM_20948.*

    Where could the problem come from??
    @fralbo:

    Welcome to the forum. There are lots of people here who truly want to help if/when/where we can.

    You didn't show your complete source code (see Forum Rule at the top of every forum page). It's much easier to provide help if your same runtime environment can be reproduced exactly.

    What Teensy model & at what speed & with what code optimization level ?? It's much easier to provide targeted help if your same hardware environment can be reproduced exactly.

    What version of the Arduino IDE (latest is 1.8.19) ?? What version of TeensyDuino (latest is 1.57) ?? It's much easier to provide targeted help if your same development environment can be reproduced exactly.

    What variable type is "cycle" defined as (int, bool, etc.) ?? How did you define "CYCLE_TIME" (#define, const int, etc.) ??

    Do you have any other interfaces defined/activated/running (I can't tell for sure from your description whether you have applied the ICM_20948 support only for the ESP32, or for your Teensy as well) ?? What library/libraries are you including ??

    If you are simply toggling a pin for this test, you could try using "fastDigitalWrite(pin, level);" where "pin" & "level" are appropriately defined.

    I recognize that these are all stabs in the dark. Better help might be possible with more details . . .

    Mark J Culross
    KD5RXT

  4. #4
    Junior Member
    Join Date
    Aug 2022
    Posts
    11
    thanks Chris, digitalWriteFast() works fast !?!!!

    Anyway, I added my code an I fall back in a 9 ms loop!!
    Is there an i2c readFast function ???
    Sorry but I find this slow execution very strange for a so powerfull MCU!

    Code:
    void loop() {
      int servoValue = 0;
    
        cycle = !cycle;
        digitalWriteFast(CYCLE_TIME, cycle);
    
        if (myICM.dataReady())
        {
          myICM.getAGMT();              // The values are only updated when you call 'getAGMT'
          int deltaSample = micros() - sampleTime;
          sampleTime = micros();
          
          lastRoll = roll;
          roll += -((myICM.gyrY() - gyrYOffset) * ((float)deltaSample / 1000000.0));
          derivRoll = abs((roll - lastRoll) / (float)deltaSample * 100000.0);
    
          // Vertical angle correction
          // If not angle variation, maybe straight line, increase time 
          // Else angle changes so in turns, reset straight line detection.
          if (abs(derivRoll) < STRAIGHT_ANGLE_DETECTION) {
            straightDriveTime++;
          } else {
            //digitalWrite(CORRECTION_ACTIVE, LOW);
            straightDriveTime = 0;
            correctionFactor = baseCorrectionFactor;
          }
    
          // If staight drive detected, active 0 correction
          if(straightDriveTime > STRAIGHT_DRIVE_TIME) {
            //digitalWrite(CORRECTION_ACTIVE, HIGH);
            straight_phase = 10;
            roll /= correctionFactor;
            correctionFactor = (correctionFactor < 2.0 ? correctionFactor + correctionFactorIncrement : correctionFactor);
          } else {
            straight_phase = 0;
          }
    
          //servoValue = SetServoValue(roll);
          //Serial.println(roll);
        }
    }

  5. #5
    Junior Member
    Join Date
    Aug 2022
    Posts
    11
    @Mark, sorry didn't pay attention to your post.

    This my complete code. I'm working on Arduino IDE 2 with TeensyDuino 1.57.
    The Teensy board is this one https://www.robot-maker.com/shop/car...nsy&results=10
    I'm using default faster optimization and speed is 600 MHz

    Code:
    /****************************************************************
     * 
     * ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
     * ** We are grateful to InvenSense for sharing this with us.
     * 
     * ** Important note: by default the DMP functionality is disabled in the library
     * ** as the DMP firmware takes up 14301 Bytes of program memory.
     * ** To use the DMP, you will need to:
     * ** Edit ICM_20948_C.h
     * ** Uncomment line 29: #define ICM_20948_USE_DMP
     * ** Save changes
     * ** If you are using Windows, you can find ICM_20948_C.h in:
     * ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
     *
     * Please see License.md for the license information.
     *
     * Distributed as-is; no warranty is given.
     ***************************************************************/
    /***************************************************************
     * V1.1 : 
    
     */
    
    #include <Arduino.h>
    #include <ICM_20948.h> // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
    #include <PWMServo.h>
    
    //#include <filters.h>
    
    #define DEBUG
    #define Serial Serial
    #define AD0_VAL 1      // The value of the last bit of the I2C address.                \
                           // On the SparkFun 9DoF IMU breakout the default is 1, and when \
                           // the ADR jumper is closed the value becomes 0
    
    // PINOUT 
    
    #define ICM_SDA                   18
    #define ICM_SCL                   19
    #define OLED_SDA                  17
    #define OLED_SCL                  16
    #define SERVO_PWM                 2
    #define CYCLE_TIME                3
    //#define CORRECTION_ACTIVE         4
    
    #define ANGLE_90                  1500
    #define ANGLE_MIN                 500
    #define ANGLE_MAX                 2500
    #define STRAIGHT_DRIVE_TIME       4000
    #define STRAIGHT_ANGLE_DETECTION  2       // Derivative value for stable angle detection
    
    
    // OLED
    //#define USE_DISPLAY
    #define SCREEN_WIDTH 128 // OLED display width, in pixels
    #define SCREEN_HEIGHT 64 // OLED display height, in pixels
    //Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire1);
    
    
    ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
    PWMServo myServo;
    int8_t blinkState;
    // Logging timer interrupt
    
    float accXOffset = -51.662448883;
    float accYOffset = -7.584472656;
    float accZOffset = 1007.918518066;
    float gyrXOffset = -001.755664229;
    float gyrYOffset = -000.003526719;
    float gyrZOffset = 000.282961845;
    float roll,lastRoll, derivRoll, leanAngle, fGyrY;
    int sampleTime, straightDriveTime, servoValue;
    volatile int8_t gotSampleInterrupt;
    float correctionFactor          = 1.0;
    float baseCorrectionFactor      = 1.000;
    float correctionFactorIncrement = 0.0001;
    int8_t straight_phase, record, dump;
    int recordIdx;
    bool wifiOK,SDOK, cycle;
    
    // Filters
    /*
    const float cutoff_freq   = 5.0;  //Cutoff frequency in Hz
    const float sampling_time = 0.001; //Sampling time in seconds.
    IIR::ORDER  order  = IIR::ORDER::OD3; // Order (OD1 to OD4)
    */
    float getInitialAccelerometerAngle();
    int SetServoValue(float roll);
    void Calibrate();
    void printFormattedFloat(float val, uint8_t leading, uint8_t decimals);
    
    bool Init_ICM20948() {
    
      Wire.setClock(1000000);
      Wire.begin();
    
      //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
    
      bool initialized = false;
      while (!initialized)
      {
    
        // Initialize the ICM-20948
        // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
        myICM.begin(Wire, AD0_VAL);
      
        //Serial.print(F("Initialization of the sensor returned: "));
        //Serial.println(myICM.statusString());
      
        if (myICM.status != ICM_20948_Stat_Ok)
        {
          Serial.println(F("Trying again..."));
          delay(500);
        }
        else
        {
          initialized = true;
        }
      }
    
      // Here we are doing a SW reset to make sure the device starts in a known state
      myICM.swReset();
      if (myICM.status != ICM_20948_Stat_Ok)
      {
        Serial.print(F("Software Reset returned: "));
        Serial.println(myICM.statusString());
      }
      delay(250);
    
      // Now wake the sensor up
      myICM.sleep(false);
      myICM.lowPower(false);
    
      // The next few configuration functions accept a bit-mask of sensors for which the settings should be applied.
    
      // Set Gyro and Accelerometer to a particular sample mode
      // options: ICM_20948_Sample_Mode_Continuous
      //          ICM_20948_Sample_Mode_Cycled
      myICM.setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
      if (myICM.status != ICM_20948_Stat_Ok)
      {
        Serial.print(F("setSampleMode returned: "));
        Serial.println(myICM.statusString());
      }
    
      // Set full scale ranges for both acc and gyr
      ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
    
      myFSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
                      // gpm2
                      // gpm4
                      // gpm8
                      // gpm16
    
      myFSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
                        // dps250
                        // dps500
                        // dps1000
                        // dps2000
    
      myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
      if (myICM.status != ICM_20948_Stat_Ok)
      {
        Serial.print(F("setFullScale returned: "));
        Serial.println(myICM.statusString());
      }
    
      // Set up Digital Low-Pass Filter configuration
      ICM_20948_dlpcfg_t myDLPcfg;    // Similar to FSS, this uses a configuration structure for the desired sensors
      myDLPcfg.a = acc_d5bw7_n8bw3; // (ICM_20948_ACCEL_CONFIG_DLPCFG_e)
                                      // acc_d246bw_n265bw      - means 3db bandwidth is 246 hz and nyquist bandwidth is 265 hz
                                      // acc_d111bw4_n136bw
                                      // acc_d50bw4_n68bw8
                                      // acc_d23bw9_n34bw4
                                      // acc_d11bw5_n17bw
                                      // acc_d5bw7_n8bw3        - means 3 db bandwidth is 5.7 hz and nyquist bandwidth is 8.3 hz
                                      // acc_d473bw_n499bw
    
      myDLPcfg.g = gyr_d5bw7_n8bw9; // (ICM_20948_GYRO_CONFIG_1_DLPCFG_e)
                                        // gyr_d196bw6_n229bw8
                                        // gyr_d151bw8_n187bw6
                                        // gyr_d119bw5_n154bw3
                                        // gyr_d51bw2_n73bw3
                                        // gyr_d23bw9_n35bw9
                                        // gyr_d11bw6_n17bw8
                                        // gyr_d5bw7_n8bw9
                                        // gyr_d361bw4_n376bw5
    
      myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myDLPcfg);
      if (myICM.status != ICM_20948_Stat_Ok)
      {
        Serial.print(F("setDLPcfg returned: "));
        Serial.println(myICM.statusString());
      }
    
      // Choose whether or not to use DLPF
      // Here we're also showing another way to access the status values, and that it is OK to supply individual sensor masks to these functions
      ICM_20948_Status_e accDLPEnableStat = myICM.enableDLPF(ICM_20948_Internal_Acc, true);
      ICM_20948_Status_e gyrDLPEnableStat = myICM.enableDLPF(ICM_20948_Internal_Gyr, true);
      Serial.print(F("Enable DLPF for Accelerometer returned: "));
      Serial.println(myICM.statusString(accDLPEnableStat));
      Serial.print(F("Enable DLPF for Gyroscope returned: "));
      Serial.println(myICM.statusString(gyrDLPEnableStat));
    
      return true;
    }
    
    void taskICM(void *param) {
      int servoValue = 0;
    
      //for (;;) {
        cycle = !cycle;
        digitalWriteFast(CYCLE_TIME, cycle);
    
        if (myICM.dataReady())
        {
          myICM.getAGMT();              // The values are only updated when you call 'getAGMT'
          int deltaSample = micros() - sampleTime;
          sampleTime = micros();
          
          lastRoll = roll;
          roll += -((myICM.gyrY() - gyrYOffset) * ((float)deltaSample / 1000000.0));
          derivRoll = abs((roll - lastRoll) / (float)deltaSample * 100000.0);
    
          // Vertical angle correction
          // If not angle variation, maybe straight line, increase time 
          // Else angle changes so in turns, reset straight line detection.
          if (abs(derivRoll) < STRAIGHT_ANGLE_DETECTION) {
            straightDriveTime++;
          } else {
            //digitalWrite(CORRECTION_ACTIVE, LOW);
            straightDriveTime = 0;
            correctionFactor = baseCorrectionFactor;
          }
    
          // If staight drive detected, active 0 correction
          if(straightDriveTime > STRAIGHT_DRIVE_TIME) {
            //digitalWrite(CORRECTION_ACTIVE, HIGH);
            straight_phase = 10;
            roll /= correctionFactor;
            correctionFactor = (correctionFactor < 2.0 ? correctionFactor + correctionFactorIncrement : correctionFactor);
          } else {
            straight_phase = 0;
          }
    
          //servoValue = SetServoValue(roll);
          //Serial.println(roll);
        }
      //} // For ever   
    
    }
    
    void setup()
    {
    
      pinMode(CYCLE_TIME,OUTPUT);  //cycle time measure
      pinMode(SERVO_PWM,OUTPUT);  //Servo PWM
    //  pinMode(CORRECTION_ACTIVE,OUTPUT);  //Correction MODE ACTIVE
      
      Serial.begin(115200); // Start the serial console
      while (Serial.available()) // Make sure the serial RX buffer is empty
        Serial.read();
    
      delay(100);
    
    #ifdef USE_DISPLAY
      Init_OLED();
    #endif
    
      Init_ICM20948();
    
      // Initialize Servo
      myServo.attach(SERVO_PWM);
      //myServo.writeMicroseconds(ANGLE_90);
      myServo.write(90);
      
      Serial.println();
      Serial.println(F("Configuration complete!"));
      
      sampleTime = micros();
    
      // Initial angle measurement
    
      roll = getInitialAccelerometerAngle();
      SetServoValue(roll);
    
    }
    
    void loop()
    {
    /*
      if (Serial.available() > 0) {
        int income;
        income = Serial.read();
        switch (income) {
           case 'c':
           case 'C':
            Calibrate();
            break;
        }
      }
    */
      taskICM(NULL);
    }
    
    int SetServoValue(float roll) {
      int servoValue;
      //servoValue = ANGLE_90 + int ((roll * 1000.0) / 90.0);
      //servoValue = servoValue < ANGLE_MIN ? ANGLE_MIN : servoValue;
      //servoValue = servoValue > ANGLE_MAX ? ANGLE_MAX : servoValue;
      
      servoValue = (int) 90 + (roll+0.5);
      //myServo.writeMicroseconds(servoValue);
      myServo.write(servoValue);
    
      return servoValue;
    }
    
    void Calibrate() {
      float meanAccX = 0, meanAccY = 0,meanAccZ = 0,meanGyrX = 0,meanGyrY = 0,meanGyrZ = 0;
      int nbSamples = 500;
    
      Serial.println("Computing offsets...");
      delay(2000);
      for (int i = 0; i < nbSamples; i++) {
        if (myICM.dataReady())
        {
          myICM.getAGMT();              // The values are only updated when you call 'getAGMT'
          meanAccX += myICM.accX();
          meanAccY += myICM.accY();
          meanAccZ += myICM.accZ();
          meanGyrX += myICM.gyrX();
          meanGyrY += myICM.gyrY();
          meanGyrZ += myICM.gyrZ();
          delay(5);
        }
      }
      meanAccX /= nbSamples;
      meanAccY /= nbSamples;
      meanAccZ /= nbSamples;
      meanGyrX /= nbSamples;
      meanGyrY /= nbSamples;
      meanGyrZ /= nbSamples;
      
      accXOffset = meanAccX;
      accYOffset = meanAccY;
      accZOffset = meanAccZ;
      gyrXOffset = meanGyrX;
      gyrYOffset = meanGyrY;
      gyrZOffset = meanGyrZ;
      
      Serial.print(" AccX offset : ");
      printFormattedFloat(accXOffset, 3, 9);
      Serial.print(" AccY offset : ");
      printFormattedFloat(accYOffset, 3, 9);
      Serial.print(" AccZ offset : ");
      printFormattedFloat(accZOffset, 3, 9);
      Serial.print(" GyrX offset : ");
      printFormattedFloat(gyrXOffset, 3, 9);
      Serial.print(" GyrY offset : ");
      printFormattedFloat(gyrYOffset, 3, 9);
      Serial.print(" GyrZ offset : ");
      printFormattedFloat(gyrZOffset, 3, 9);
      Serial.println();
      delay(5000);
      roll = 0.0;
    }
    
    /**
     * Compute Angle from Acceleration vectors
     * That only works if the motorbike is in straight line  !!!
     */
    float getInitialAccelerometerAngle()
    {
      float acc_total_vector = 0.0;
      float angleY = 0.0;
      
      
      while (!acc_total_vector) {
        while (!myICM.dataReady()) {
          delay(5);
        }
        myICM.getAGMT();
        // Calculate total 3D acceleration vector : √(X + Y + Z)
        acc_total_vector = sqrt(pow(myICM.accX(), 2) + pow(myICM.accY(), 2) + pow(myICM.accZ(), 2));
      }
      
      // To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
      if (abs(myICM.accY()) < acc_total_vector) {
        angleY = asin((float)myICM.accX() / acc_total_vector) * (180 / PI);
      }
      return -angleY;
    }
    
    void printFormattedFloat(float val, uint8_t leading, uint8_t decimals)
    {
      float aval = abs(val);
      if (val < 0)
      {
        Serial.print("-");
      }
      else
      {
        Serial.print(" ");
      }
      for (uint8_t indi = 0; indi < leading; indi++)
      {
        uint32_t tenpow = 0;
        if (indi < (leading - 1))
        {
          tenpow = 1;
        }
        for (uint8_t c = 0; c < (leading - 1 - indi); c++)
        {
          tenpow *= 10;
        }
        if (aval < tenpow)
        {
          Serial.print("0");
        }
        else
        {
          break;
        }
      }
      if (val < 0)
      {
        Serial.print(-val, decimals);
      }
      else
      {
        Serial.print(val, decimals);
      }
    }
    Last edited by fralbo; 08-21-2022 at 07:24 PM. Reason: cleanup

  6. #6
    Senior Member
    Join Date
    Apr 2020
    Location
    DFW area in Texas
    Posts
    511
    @fralbo:

    OK, thanks for your recent posts. Unfortunately, I don't have any experience with the ICM_20948, so I can't offer any specific recommendations. In general, if I were attacking the problem, I'd start by commenting out most of the calls in the loop(), in an attempt to narrow down where your time is being spent. Once you've identified the major offender(s), I'd take a look at the library code, & maybe do some commenting there as well. Hopefully, you can pin down specifically what is eating up your time.

    Good luck & feel free to ask more questions if/as needed.

    Mark J Culross
    KD5RXT

  7. #7
    Senior Member
    Join Date
    Nov 2012
    Posts
    1,900
    You aren't timing the Teensy. You are timing how long it takes to fetch the data from the ICM_20948 using I2C protocol.

    Pete

  8. #8
    Junior Member
    Join Date
    Aug 2022
    Posts
    11
    `myICM.getAGMT()` is the problem, it read 23 bytes using the Wire library.
    Cannot figure out why it's so long on this board.

  9. #9
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    16,503
    It isn't going long so much as OFTEN - and taking the time needed for the i2c transfer - that is not fast.

    An empty loop on T_4.0 runs at 12M or MORE cycles per second - with just a counter.

    Using the IDE on an ESP32 they are lucky to do 200K cycles per second - maybe more if both cores are brought in - but not that can generally be seen.

    Any added code runs that often and the longer it takes the more is slows down.

    If the i2c transfer is only needed 5 or 20 times per second - use an elapsedMillis or something to limit calling that selectively rather than each loop

  10. #10
    Junior Member
    Join Date
    Aug 2022
    Posts
    11
    I found!
    despite wht I read somewhere, `Wire.setClock(1000000);` has to be called AFTER `Wire.begin();` and not before!
    Now I have a 1 ms loop time, which is what I expected.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •