Teensy 3.x multithreading library first release

Hi @ftrias

I incorporated the sensor threads into my rover program and have an interesting issue that cropped up using a teensy 3.5. When I just used Serial to check the whether the threads run and receiving good data everything worked fine. When is changed to using Serial4, which I have a 915mhz telemetry radio attached (knockoff of the 3dr radio) the threads don't start and radio goes crazy, connects, disconnects and the threads don't start. This occurs pretty much right after I power up and the sketch starts. On startup it sends some messages that the USB host shield library, BNO055 init status and thread status to Serial4. Any ideas would be appreciated.

thanks
Mike
 
Last edited:
Hi @ftrias

I incorporated the sensor threads into my rover program and have an interesting issue that cropped up using a teensy 3.5. When I just used Serial to check the whether the threads run and receiving good data everything worked fine. When is changed to using Serial4, which I have a 915mhz telemetry radio attached (knockoff of the 3dr radio) the threads don't start and radio goes crazy, connects, disconnects and the threads don't start. This occurs pretty much right after I power up and the sketch starts. On startup it sends some messages that the USB host shield library, BNO055 init status and thread status to Serial4. Any ideas would be appreciated.

thanks
Mike

Not sure what the problem is but not all libraries are thread safe. Any code that uses global variables cannot be used on two threads at the same time. That may or may not be the problem.

Perhaps if you put up all your code and the exact error messages you are getting it, I might have more suggestions.
 
@ftrias. Think one of the problems was that I was trying to print some debugging data from the sensors but the speed was faster than the radio was set for (57K) so it was overloaded. I changed that and the sensor data looks ok but now I some other problem with the code using the data - maybe a timing problem between data available and code running. I have to experiment a little more.

Would you recommend using volatiles for the global variables I am using in the threads? Do you want me to keep you all posted?
 
HTML:
@ftrias. Sorry for not getting back to you on this but I started on another project while trying to debug my challenge with the rover. After I put in a bunch of serial.print statements what I am seeing is that after two or three sensor "reads" (reads in my case means using the data from the threads one of the sonar sensor values will be erroneous. By this I mean, if I hold the rover in my hand a fixed distance from the wall I would get something like 56, 53, 62, 50 cm from the 4 sensors. What happens is that I will get what I am going drop outs and the sensor values will be off, e.g., 0, 2, 70, 48 or 8, 41, 55, 48 cm. I tried changing the timeslices and threads.delay to delay. I am thinking this is a timing issue with the sonar sensors? Any suggestions?

This is how I have the threads setup
Code:
    id1 = threads.addThread(sonar_thread);
    id2 = threads.addThread(sharp_dist_thread);
    id3 = threads.addThread(bno055_thread);
    threads.setTimeSlice(0, 1);
    threads.setTimeSlice(id1, 50);
    threads.setTimeSlice(id2, 10);
    threads.setTimeSlice(id3, 1);
    if (threads.getState(id1) == Threads::RUNNING) telem.println("Sonar thread started");
    if (threads.getState(id2) == Threads::RUNNING) telem.println("IR thread started");
    if (threads.getState(id3) == Threads::RUNNING) telem.println("BNO055 thread started");

This is how I am using the data from the thread (whole obstacle avoidance module:
Code:
void decide_direction() {
  //interrupts();

  if(sensorTimer1 > sensorUpdate1){
     telem << "Entering Decide Direction Test" << endl;
     update_sensors();
     sensorTimer1 = 0;
  }
  
  // No forward obstacles
  if(hds > fowardheadThreshold && lls > sideSensorThreshold && 
        lcs > lcThreshold && lrs > sideSensorThreshold &&
        irF > lcIRthreshold) {
    nextMove = "Straight";
    //telem << "(DC) Next Move Straight" << endl;
  }
  
  // If everything is blocked in the forward direction lets backupSensorThreshold
  // and run the Bubble rebound/VFH routing
  else if(lls < sideSensorThreshold && lrs < sideSensorThreshold && 
      (hds < fowardheadThreshold || lcs < lcThreshold 
      || irF < lcIRthreshold)) {
    
    moveBackward();
   
    nextMove = "RunBubble";
    //telem << "(DC) Everything blocked Next Move Backup" << endl;
  }
  
  // Special Cases
  
  else if(lls < sideSensorThreshold && lrs == MAX_DISTANCE && 
      hds < fowardheadThreshold && lcs < lcThreshold 
      && irF < lcIRthreshold) {

    //moveBackward();
    //nextMove = "RunBubble";
	
    //nextMove = "Left";
    //turn_time = left_45;
    nextMove = "Right";
    turn_time = right_45;

    //telem << "(DC) Everything blocked Next Move Backup" << endl;
  }

  else if(lrs < sideSensorThreshold && lls == MAX_DISTANCE && 
      hds < fowardheadThreshold && lcs < lcThreshold 
      && irF < lcIRthreshold) {

//    moveBackward();  
//    nextMove = "RunBubble";

    //nextMove = "Right";
    //turn_time = right_45;
    nextMove = "Left";
    turn_time = left_45;
	
    //telem << "(DC) Everything blocked Next Move Backup" << endl;
  } 
  
  // Do any of the front facing range sensors detect an obstacle closer than their
  // threshold?  If so, then prepare to turn left or right. hds < fowardheadThreshold ||
  //else if( lcs < lcThreshold || irF < lcIRthreshold)
    
  // If head sensor is blocked (less than threshold) run bubble and be done with iter_swap
  // else if(hds < fowardheadThreshold) {
  //  nextMove = "RunBubble";
  //}
  
  else if(irF < lcIRthreshold || lcs < fowardheadThreshold
          || hds < fowardheadThreshold) 
		{
			//moveBackward();  
			//nextMove = "RunBubble";

			if(lls < lrs) {       //If left  is greater than right distance move left
				//nextMove = "Left";
				//turn_time = left_45;    //was 37
				nextMove = "Right";
				turn_time = right_45;				
				//telem << "(DC) Next Move is left (Center Blocked)" << endl;
				//mLeft();
				//delay(turn_time);     //was 1500, 700, 225 - calc at 275 change to 325
				//mStop();
				//nextMove = "Straight";
			} else if(lls > lrs){    //If right is greater than left distance move right
				//nextMove = "Right";
				//turn_time = right_45; //was 37
				nextMove = "Left";
				turn_time = left_45; //was 37
				//telem << "(DC) Next Move is right (Center Blocked)" << endl;
				//mRight();
				//delay(turn_time);     //was 1500, 700, 225 - calc at 275 change to 325
				//mStop();
				//nextMove = "Straight";
			} else {            //if all else fails run bubble band algorithm
				nextMove = "RunBubble";
				//telem << "(DC) Next Move Determined by bubble [fwd sensors blocked - backup first]" << endl;

			}
  }

  // What about the angled looking  detectors?
  // If right and center distances is less than threshold move left alot
  else if(lls < sideSensorThreshold && lcs < fowardheadThreshold){
    //nextMove = "Left";
    //turn_time = left_57;
	  nextMove = "Right";
	  turn_time = right_57;
    //telem << "(DC) Next Move alot Left" << endl;
  }
  
  // If left and center distances is less than threshold move right alot
  else if(lrs < sideSensorThreshold && lcs < fowardheadThreshold){
    //nextMove = "Right";
    //turn_time = right_57;
    nextMove = "Left";
    turn_time = left_57;
    //telem << "(DC) Next Move alot Right" << endl;
  }
  
  // If right distances is less than threshold and center not blocked 
  // move left a little
  else if (lls < sideSensorThreshold && lcs > fowardheadThreshold)
  {
    //nextMove = "Left";
    //turn_time = left_37;
    nextMove = "Right";
    turn_time = right_37;
    //telem << "(DC) Next Move Left" << endl;
  }
  //
  // If left distance is less than threshold and center not blocked 
  // move right a little
  //
  else if(lrs < sideSensorThreshold && lcs > fowardheadThreshold)
  {
    //nextMove = "Right";
    //turn_time = right_37;
    nextMove = "Left";
    turn_time = left_37;
    //telem << "(DC) Next Move Right" << endl;
  }
  
  // If we can't go forward, stop and take the appropriate evasive action.
  if (nextMove != "Straight")
  {
    // lets stop and figure out what's next
    mStop();
    set_speed(turnSpeed);

    //telem << lastMove << ", " << nextMove << endl << endl;
    
    if(lastMove == "Right" && nextMove == "Left") {
      nextMove == "Right";
      turn_time = right_57;
      turnCounter = turnCounter + 1;
      //telem << "(OSC) Next Move Right" << endl;
	  
     } else if(lastMove == "Left" && nextMove == "Right") {
      nextMove == "Left";
      turn_time = left_57;
      turnCounter = turnCounter + 1;
      //telem << "(OSC) Next Move Left" << endl;
      }
      
    if(turnCounter != 0 && nextMove != "RunBubble"){
      nextMove = lastMove;
    }
    
    //telem << "Actual Move: " << nextMove << endl << endl;
    
    turn_timer = 0;
	
    if (nextMove == "Right") {
		  mRight();
      gDirection = DIRECTION_RIGHT;
		  smartDelay(turn_time);     //was 1500, 700, 225 - calc at 275 change to 325
		  mStop();
		  send_telemetry();
	  } else if (nextMove == "Left") { 
        gDirection = DIRECTION_LEFT;
        mLeft();
        smartDelay(turn_time);     //was 1500, 700, 225 - calc at 275 change to 325
        mStop();
        send_telemetry();
       } else if (nextMove == "RunBubble") {
			    turnCounter = 0;
			    mBackward();
			    smartDelay(200);
			    mStop();
			    Select_Direction();
       }

    lastMove = nextMove;    
    
    } else {
		// If no obstacles are detected close by, keep going straight ahead.
		lastMove = "Straight";
    
		set_speed(speed);
    		//reset counters after clearing obstacle
		turnCounter = 0;
    
		mForward();
		//keep moving forward until next obstacle is found
		while(hds > fowardheadThreshold && lls > sideSensorThreshold && 
          lcs > lcThreshold && lrs > sideSensorThreshold &&
          irF > lcIRthreshold && stasis_flag == 0) {


			if(telem.available() > 0) {
				int val = telem.read();  //read telem input commands  
				if(val == 't') {
					//telem.println("toggle Roam Mode");
          //noInterrupts();
					toggleRoam();
					return;
				}
			}
			  //telem << "(DC) End of Tests Updating Sensors" << endl << endl;  

			  //Send telemetry data to radio
			  gDirection = DIRECTION_FORWARD;
        getTicks();
			  send_telemetry();

        if(sensorTimer > sensorUpdate){
          telem << "While Loop for Going Straight" << endl;
          update_sensors();
          sensorTimer = 0;
        }
 
			  //read_sensors();   
			  //oneSensorCycle();
		  }   
		  mStop();
      if(stasis_flag == 1) stasis_correction();
    }
}

Code:
void update_sensors(){
  lls = cm[0];
  lcs = cm[1];
  lrs = cm[2];
  hds = cm[3];
  irF = frtIRdistance;
  irR = rearIRdistance;

  telem << lls << ", " << lcs << ", " << lrs << ", " << hds << endl;
  telem << irF << ", " << irR << endl;
}
 
Last edited:
HTML:
@ftrias. Sorry for not getting back to you on this but I started on another project while trying to debug my challenge with the rover. After I put in a bunch of serial.print statements what I am seeing is that after two or three sensor "reads" (reads in my case means using the data from the threads one of the sonar sensor values will be erroneous. By this I mean, if I hold the rover in my hand a fixed distance from the wall I would get something like 56, 53, 62, 50 cm from the 4 sensors. What happens is that I will get what I am going drop outs and the sensor values will be off, e.g., 0, 2, 70, 48 or 8, 41, 55, 48 cm. I tried changing the timeslices and threads.delay to delay. I am thinking this is a timing issue with the sonar sensors? Any suggestions?

@mjs513, it does seem there are timing issues. If I understand the sonar library, it sends out a "ping" and then counts the time until the ECHO pin changes to mark that it heard a return "ping". If while it's waiting execution goes to another thread, the "ping" returns, but it won't know this until execution returns to the listening thread. When execution returns to the listening thread, it will think the "ping" has just returned and record the time. Because the timing resolution required to detect such short distances is in the microsecond range, if your thread suspends for 50 milliseconds, this could throw everything off. That's one theory.

Threading may not be ideal for this situation. Perhaps you should use interrupts (see attachInterrupt()) on the ECHO pins.
 
@ftrias. I just tried to use setMircoseconds to shorten the time with one combination of initial values it worked ok for awhile. Think the bottom line is for something like sonar sensors or for that matter time of flight sensors if you use a timer to measure the values you are going to have difficulties. Can't think a way around this. Thanks for help. May try an interrupt method but we'll see.
 
can this be added to current code without side effects if someone were to use an extra thread? my sketch is 70k+ in size with 8k sram used, or will i have to reconfigure everything?
 
can this be added to current code without side effects if someone were to use an extra thread? my sketch is 70k+ in size with 8k sram used, or will i have to reconfigure everything?

You should be able to just add it. The only problem you may encounter is if you use the same resources in different threads. For example, if you use Serial in two different threads, it won't work.
 
hardware access yes, but i figure for blocking libraries to throw them in their own thread, like easyvr for example1-2sec blocking for listening on uart port, itll be the only one accessing the port anyways

this could be very good for canbus networking while leaving other threads to process regular work, but for shared spi busses or i2c busses, i can see where this can fail, ( if 2 or more threads access the shared i2c/spi bus, corruption will definately be done. im testing one on a uart lcd @ 625,000 baud and its working great, update random digits on the screen, i added 2 extra threads as well, blinking led with 1sec delay between blinks, and another for scrolling numbers in debug screen, scrolls way faster now than before considering the 14-44ms latency on the display's ACK/NAK response

Tony
 
Last edited:
i dont know if this is a library issue or a multithreading issue
can 2 instances be used in separate threads for the same library?
it did not work for my tests, each lcd instance has it's own serial port, maybe the library cant handle 2 accesses at same time?

also one more question, might be interesting...

if we set a volatile uint8_t idleBus variable for a multiple I2C device bus, if we are to use talking to the same bus from both "threads" couldnt they validate the state of the uint8_t before accessing the bus, then releasing the byte back to 0 to show other thread the bus is available? example, if the uint8_t was set the other thread wouldnt be able to access that bus until the uint8_t was set back to 0.

or, perhaps,

Code:
while ( idleBus );

idleBus = 1; // starting i2c traffic

// do some traffic work //

idleBus = 0; // your done with the i2c bus, release it for other code
the other thread would block until it is able to get it's instructions sent over i2c, it's better no? gives priority to certain I2C peripherals
 
Last edited:
Just to let you know it is working great here.
I'm actually erasing SerialFlash files (writing 0xFF) in a different thread.
Thanks!
 
i got the library to play nice by changing the private to protected in the class, now im able to run instances in different threads, good to know for library multithreading ...

any idea if the volatile uint8_t idea would be good ? i have 11 i2c devices on the bus and would rather have 1 or 2 a higher priority, setting the variable would prevent other code from accessing the bus no? or is there a timing issue?
 
Pre-emptive right, worst case after "while ( idleBus );" above exits could there be a task switch ? If so then "idleBus = 1;' could end up compromised if another thread jumped in and actually put the bus to work first?

Is this valid overkill?:
Code:
bool mine=false;
while ( !mine ) {
  if ( !idlebus ) {
    idleBus++;
    if ( 1 < idleBus ) idleBus--;
    else mine = true;
  }
}
// do some traffic work //
idleBus--; // your done with the i2c bus, release it for other code

Of course if a thread did "idleBus++;" and lost focus before getting to utilize it - the i2c would be locked out until it returned to use it.
 
Last edited:
sorry i didnt understand, i understood it could switch once the while loop broke, but the threading might seitch too fast, are you saying its overkill the volatile method or that wont work either?
 
I may be assuming too much switching. Once you get the focus is it true that nothing else { no user code on i2c } runs until you lose focus for some time 'x'?

So if you hit your post #86 code and idlebus==1, your time slice will end without it changing. And if you re-enter and find 0==idlebus then that time slice will last - without interruption - for enough cycles to safely take ownership of idlebus?
 
i have no idea, i had 128 gpios on the spi bus and i put the lcd code in a thread, forgot about the spi commands in its handler, and disaster struck lol, anyways, if there is a recommended way of handling device hardware between threads im up for input, i guess i could bench test 1 device on the i2c bus when i get home and try out if 2 threads may be able to access it using a volatile bool for ready or not availability, but if it doesnt work, id like to know if theres a better idea that i could try when i get home later, i will probably try out a mcp23017, set a bank to output and another bank to input, and have threadA check 1 bank and threadB check the other bank and see if they properly read without messing up any print debugging

thanks
tony
 
Last edited:
Won't hurt to try it ... If the task switch is preemptive but for a fixed time slice then your idea should work - once you see ZERO and set the volatile ONE it should work - if all threads follow the same rule. If it works - worst case you'll waste a time slice ( or more ) waiting for your turn on the bus - and 'safely' block anyone else from using it when your time is up if you are not complete.
 
Is this valid overkill?:
Code:
bool mine=false;
while ( !mine ) {
  if ( !idlebus ) {
    idleBus++;
    if ( 1 < idleBus ) idleBus--;
    else mine = true;
  }
}
// do some traffic work //
idleBus--; // your done with the i2c bus, release it for other code
No, this is invalid underkill. :p Trying to invent your own locking scheme has a 99% chance of failure.

Doing math on volatile variables is not atomic, so schemes like yours are bound to fail.

A little test sketch:
Code:
#include <TeensyThreads.h>

volatile uint32_t ctr = 0;

volatile int idleBus = 0;

void thread_fn() {
    while(true) {
        bool mine=false;
        while ( !mine ) {
            if ( !idleBus ) {
                idleBus++;
                if ( 1 < idleBus ) idleBus--;
                else mine = true;
            }
        }
        // do some traffic work //
        ctr++;
        
        idleBus--;
    }
}

void setup() {
    Serial.begin(9600);
    delay(2000);
    Serial.println("Starting");

    threads.setTimeSlice(0, 1);
    auto thread_id1 = threads.addThread(thread_fn);
    threads.setTimeSlice(thread_id1, 1);
    auto thread_id2 = threads.addThread(thread_fn);
    threads.setTimeSlice(thread_id2, 1);

    elapsedMillis print_timer;

    uint32_t counter_prev = 0;

    while(true) {
        if(print_timer > 1000) {
            Serial.print("ctr: ");
            Serial.print(ctr);
            Serial.print(" idleBus: ");
            Serial.println(idleBus);

            print_timer = 0;
        }
        else threads.yield();
    }
}

void loop() {}

This will result in 'idleBus == -1' and the threads looping infinitely trying unsuccessfully to acquire the 'idleBus' lock.

There is a Mutex class, use that. As a bonus, you won't waste CPU time on busy-waiting for the lock.

Code:
#include <TeensyThreads.h>

volatile uint32_t ctr = 0;

Threads::Mutex lock;

void thread_fn() {
    while(true) {
        {
            Threads::Scope locker(lock);
            // locker is in scope, the lock is acquired
            
            // do some traffic work //
            ctr++;
        }
        // locker went out of scope, the lock is released
    }
}

void setup() {
    Serial.begin(9600);
    delay(2000);
    Serial.println("Starting");

    threads.setTimeSlice(0, 1);
    auto thread_id1 = threads.addThread(thread_fn);
    threads.setTimeSlice(thread_id1, 1);
    auto thread_id2 = threads.addThread(thread_fn);
    threads.setTimeSlice(thread_id2, 1);

    elapsedMillis print_timer;

    uint32_t counter_prev = 0;

    while(true) {
        if(print_timer > 1000) {
            Serial.print("ctr: ");
            Serial.println(ctr);

            print_timer = 0;
        }
        else threads.yield();
    }
}

void loop() {}
 
ill try out your mutex method when i get home in a few hours, i still dont quite get a grip on how it works so ill have to do a little research on it
thanks for the assistance
tony
 
ill try out your mutex method when i get home in a few hours, i still dont quite get a grip on how it works

Code:
    {
        Threads::Scope locker(lock);
        // locker is in scope, the lock is acquired
         
        // do some traffic work //
        ctr++;
    }
    // locker went out of scope, the lock is released
Threads::Scope is a class that locks the Mutex in the constructor and unlocks it in the destructor (locker is destroyed when you leave the block where it was constructed). This is a standard C++ technique called RAII. It's doing the same thing as this:
Code:
    lock.lock();
    ctr++;
    lock.unlock();
 
what happens if it locks a thread in middle of an i2c/spi communication and then does its work on the same bus?
 
what happens if it locks a thread in middle of an i2c/spi communication and then does its work on the same bus?
You want to hold a lock for the bus during an entire communication sequence that's not supposed to interrupted (for I2C the entire time from start to stop; for SPI the entire time from beginTransaction to endTransaction). Everything that accesses the bus must use that lock or things will not work correctly.
 
thanks tni, im starting to get familiar with the mutex lock now, just a few silly questions since this is still new to me

Threads::Scope locker(lock);

where did Scope locker come from and im guessing the lock unlocks at the end of every while cycle of the thread? (even though the while never exits?)

also can the "lock" fail to happen if something else it keeping it from happening?
 
Last edited:
thanks tni, im starting to get familiar with the mutex lock now, just a few silly questions since this is still new to me

Threads::Scope locker(lock);

where did Scope locker come from and im guessing the lock unlocks at the end of every while cycle of the thread? (even though the while never exits?)

"Threads::Scope" is a class. When an instance is created, the constructor grabs the lock given. When it is destroyed, the destructor releases the lock. The instance is created at the "{". And it is destroyed when it goes out of scope at the "}". In other cases, it would also get released (via instance destruction) if you put a "return" or "goto"or there is an exception, or something that sends the program flow outside of the scope. As @tni mentioned this is a common C++ way to do things.

The lock does not get released when the threads change. That's the idea of locking. If one thread grabs a lock, another thread cannot use it. In some cases, if you have multiple locks, this can lead to problems (called a deadlock), but it doesn't look like that's a problem here.
 
Back
Top