change thread priority from a timer callback

Discussions and support about ChibiOS/RT, the free embedded RTOS.
tridge
Posts: 141
Joined: Mon Sep 25, 2017 8:27 am
Location: Canberra, Australia
Has thanked: 10 times
Been thanked: 20 times
Contact:

change thread priority from a timer callback

Postby tridge » Fri May 04, 2018 4:48 am

In ArduPilot we have the concept of a temporary boost to a threads priority. This is used to balance the priority requirements of SPI bus threads with flight control loop threads.
To implement this we want to be able to change the priority of a thread from a chVT timer callback. This allows the thread to setup its boosted priority, then automatically lose that boost 200 microseconds later when the timer callback is called.
How do we achieve this with ChibiOS/RT?
The problems are:
- there is no API function that I can find for changing priority of a thread using its context pointer instead of assuming the current thread
- if instead we change thread->realprio directly then that seems not to trigger a reschedule on the return of the timer callback

User avatar
Giovanni
Site Admin
Posts: 14444
Joined: Wed May 27, 2009 8:48 am
Location: Salerno, Italy
Has thanked: 1074 times
Been thanked: 921 times
Contact:

Re: change thread priority from a timer callback

Postby Giovanni » Fri May 04, 2018 7:46 am

Hi,

Such an API is not present by design, changing a thread priority would not be as simple as changing the priority field. Depending on a thread state it could be:
- In the priority ordered ready list, reordering would be required.
- In another priority ordered list (semaphores, mutexes, possibly others).
- It could be the current thread.
- It could be in a FIFO list.
- It could be sleeping in another state without being in a list.

For each case a specific action should taken. Another problem is that if the thread is a priority boosted state (prio>realprio, because priority inheritance algorithm), changing priority is even harder, there are several sub-cases to consider.

What is doing this SPI thread when you want to change its priority? you may consider make the SPI thread change its own priority based on a global state adding check points in between SPI operation, that would be much faster that going through the decision tree required for a remote priority change.

Give me more details.

Giovanni

tridge
Posts: 141
Joined: Mon Sep 25, 2017 8:27 am
Location: Canberra, Australia
Has thanked: 10 times
Been thanked: 20 times
Contact:

Re: change thread priority from a timer callback

Postby tridge » Fri May 04, 2018 8:55 am

ok, trickier than I expected.

> What is doing this SPI thread when you want to change its priority? you may consider make the SPI thread change its own priority based on a
> global state adding check points in between SPI operation, that would be much faster that going through the decision tree required for a remote
> priority change.

It isn't the SPI bus threads that we want to change the priority of. The setup is this:

- there is a main thread doing attitude control, navigation etc
- there is a thread per bus (so a thread per SPI bus, plus one per i2c bus etc)

The SPI bus threads are normally the highest priority threads in ArduPilot. On some flight boards these threads can be quite busy. For example, a Cube 2.1 has 3 SPI IMUs, and two of them can run at 8k transactions per second (with each transaction transferring 14 bytes of data). The 4th runs at 1k transactions per seonond. Normally we run these as the top priority as they must drain the FIFOs from the IMUs before those FIFOs overflow, so the timing is quite tight.

However if we just run the SPI threads as top priority then the timing of the main attitude control loop isn't as good as it should be. The SPI threads can prevent the main loop from running its PID control and rate output at the right time.

The solution we have is a function that temporarily boosts the priority of the main thread at the right point so that it can do the flight critical functions. That boost lasts 200 microseconds, then it loses its priority again on a timer.

I think what I'll do is add a hook in a different place in the main thread to drop the priority back down. It will need to be a special case for HAL_ChibiOS, but it should work.

User avatar
FXCoder
Posts: 384
Joined: Sun Jun 12, 2016 4:10 am
Location: Sydney, Australia
Has thanked: 180 times
Been thanked: 130 times

Re: change thread priority from a timer callback

Postby FXCoder » Fri May 04, 2018 9:07 am

One suggestion would be to use the event system using direct thread events.
In the VT set it to pass the thread pointer for main in the callback.
Then in the VT callback signal an event directly to main thread with chEvtSignalI(main_thd, some_event_code).
Main can do a chEvtGetAndClearEvents(...) in its loop and can then drop its own priority.

tridge
Posts: 141
Joined: Mon Sep 25, 2017 8:27 am
Location: Canberra, Australia
Has thanked: 10 times
Been thanked: 20 times
Contact:

Re: change thread priority from a timer callback

Postby tridge » Fri May 04, 2018 9:26 am

FXCoder wrote:Main can do a chEvtGetAndClearEvents(...) in its loop and can then drop its own priority.


The problem is that the main thread may be in a large block of expensive maths when the timer goes off, so it isn't checking events.

I'm now working on a different approach where I'm adding a call into the AP_HAL scheduler when the critical functions are complete, and that will change the priority back if need be. In that way all the priority changes happen from within the main thread.

User avatar
Giovanni
Site Admin
Posts: 14444
Joined: Wed May 27, 2009 8:48 am
Location: Salerno, Italy
Has thanked: 1074 times
Been thanked: 921 times
Contact:

Re: change thread priority from a timer callback

Postby Giovanni » Fri May 04, 2018 11:09 am

Anyway, I will look into implementing priority change. It will be done in trunk but you can back-port the code in any version, the scheduler has been stable for years now.

Giovanni

faisal
Posts: 374
Joined: Wed Jul 19, 2017 12:44 am
Has thanked: 44 times
Been thanked: 60 times

Re: change thread priority from a timer callback

Postby faisal » Sat May 05, 2018 5:31 am

tridge wrote:The SPI bus threads are normally the highest priority threads in ArduPilot. On some flight boards these threads can be quite busy. For example, a Cube 2.1 has 3 SPI IMUs, and two of them can run at 8k transactions per second (with each transaction transferring 14 bytes of data). The 4th runs at 1k transactions per seonond. Normally we run these as the top priority as they must drain the FIFOs from the IMUs before those FIFOs overflow, so the timing is quite tight.


Wow, that's a fantastic amount of data. Do you have 2 interrupts, one at 8k and the other at 1k - and in each interrupt you perform spi polled exchanges? Do you use DMA? I'm guessing the DMA interrupt overhead may not be worth it for dozens of bytes of data. I'm working on something similar and was wondering what would be the most efficient approach - I figure you must have figured that out already :) .

User avatar
FXCoder
Posts: 384
Joined: Sun Jun 12, 2016 4:10 am
Location: Sydney, Australia
Has thanked: 180 times
Been thanked: 130 times

Re: change thread priority from a timer callback

Postby FXCoder » Sat May 05, 2018 5:33 am

Hi Andrew,
OK on the CPU heavy math issue.
BTW are you using classic tick based or tickless ChibiOS mode?

Cheers,

Bob

User avatar
Giovanni
Site Admin
Posts: 14444
Joined: Wed May 27, 2009 8:48 am
Location: Salerno, Italy
Has thanked: 1074 times
Been thanked: 921 times
Contact:

Re: change thread priority from a timer callback

Postby Giovanni » Sat May 05, 2018 6:37 am

faisal wrote: Wow, that's a fantastic amount of data. Do you have 2 interrupts, one at 8k and the other at 1k - and in each interrupt you perform spi polled exchanges? Do you use DMA? I'm guessing the DMA interrupt overhead may not be worth it for dozens of bytes of data.


Good question. Perhaps it would be a good idea to have a thread doing SPI polled exchanges instead of using DMAs from a VT callback.

I would consider a closed loop thread, using windowed delays for accurate timing and performing bursts of polled exchanges. After each exchange it could do a priority check based on the state of a static variable. Windowed delays would allow it to recover eventual lost "ticks".

Note that a fast VT probably has more overhead than a thread because the VT complex ISR (tick-less mode make it even more complex) and, in addition, the SPI ISR. Also using a GPT would improve over a VT in this role.

I see space for optimizations in this critical code.

Giovanni

User avatar
Giovanni
Site Admin
Posts: 14444
Joined: Wed May 27, 2009 8:48 am
Location: Salerno, Italy
Has thanked: 1074 times
Been thanked: 921 times
Contact:

Re: change thread priority from a timer callback

Postby Giovanni » Sat May 05, 2018 3:46 pm

Hi,

I wrote a tentative implementation of a function able to change another thread priority. I am not sure it is fully correct because not all cases have been carefully considered.

Code: Select all

/**
 * @brief   Changes another's thread priority level.
 * @note    The function returns the real thread priority regardless of the
 *          current priority that could be higher than the real priority
 *          because the priority inheritance mechanism.
 *
 * @param[in] tp        pointer to the thread
 * @param[in] newprio   the new priority level of the running thread
 * @return              The old priority level.
 *
 * @iclass
 */
tprio_t chThdSet2PriorityI(thread_t *tp, tprio_t newprio) {
  tprio_t oldprio;

  chDbgCheckClassI();

  chDbgCheck((tp != NULL) && (newprio <= HIGHPRIO));

#if CH_CFG_USE_MUTEXES == TRUE
  oldprio = tp->realprio;
  if ((currp->prio == tp->realprio) || (newprio > tp->prio)) {
    tp->prio = newprio;
  }
  tp->realprio = newprio;
#else
  oldprio = tp->prio;
  tp->prio = newprio;
#endif

  switch (tp->state) {
  case CH_STATE_READY:
    (void) chSchReadyI(queue_dequeue(tp));
    break;
#if CH_CFG_USE_SEMAPHORES_PRIORITY == TRUE
  case CH_STATE_WTSEM:
    queue_prio_insert(queue_dequeue(tp), &tp->u.wtsemp->queue);
    break;
#endif
#if CH_CFG_USE_MESSAGES_PRIORITY == TRUE
  case CH_STATE_SNDMSGQ:
    queue_prio_insert(queue_dequeue(tp), &tp->msgqueue);
    break;
#endif
  case CH_STATE_WTMTX:
    queue_prio_insert(queue_dequeue(tp), &tp->u.wtmtxp->queue);
    break;
  default:
    /* All remaining states do not require queue handling.*/
    chDbgAssert(tp->state < CH_STATE_FINAL, "invalid state");
    break;
  }

  return oldprio;
}


Do not assume it is final but you can include it in your code anyway.

Giovanni


Return to “ChibiOS/RT”

Who is online

Users browsing this forum: No registered users and 4 guests