Blog

Introduction

freeRTOS is a widely used real-time operating system (RTOS) for many different microcontrollers (MCU). There are ports from small 8-bit to 32-bit multi-core microcontrollers. freeRTOS gets along with few resources (memory, CPU), so it can be used e.g. also on different Arduino boards. For an optimal adaptation to the respective requirements and resources, the kernel can be configured very flexibly.

freeRTOS provides all the important features of an RTOS for embedded systems, such as tasks, queues, mutex, binary/counting semaphores, notifications, event groups, streams, timers, heap memory management, stack overflow checking, etc. The use of the most important elements is to be shown here on the basis of examples.

Some remarks about freeRTOS for Arduino (Uno and Mega), the boards used here:

• The boards use 8-bit microcontrollers (ATmega328P or ATmega2560): The further development of freeRTOS has been primarily focused on 32-bit MCUs for some years now, which means that some newer features are not available on ATmegas or are available in a limited way.
• The mentioned MCUs are very modestly equipped concerning memory, especially RAM. Since each task needs its own stack, and the kernel for itself and all used interprocess communication etc. also takes up RAM, memory shortage can quickly occur here. This is especially true for the Arduino Uno with only 2 Kbytes of RAM: With this, apart from small, trivial examples, small multitasking applications are actually possible.

freeRTOS implements a fixed-priority preemptive scheduling policy, with round-robin time-slicing of equal priority tasks. Task state diagram (copied from https://www.freertos.org/RTOS-task-states.html):

Tasks

Example with two tasks: Each one controls the blicking of a LED, whereby the interval time can be set separately by means of potentiometers. The two voltages set at the potentiometers are measured by the analog-to-digital converter (ADC) of the MPU ATmega328P. The read value in the range 0…1023 (10-bit resolution) is used as half period time in milliseconds for the blinking of the respective LED (25 is added to the value read in from the ADC, so that even the highest blinking frequency still remains visible: 1 / (2 * 25ms) = 20Hz.

Tasks are implemented as functions, which can then be started e.g. with xTaskCreate(). The maximum stack size specified here is 100 bytes each – this is an empirical value, which is usually sufficient for such simple task functions (in this example, probably 50 would also be sufficient, but with the double value we still have reserve). Both tasks run with a priority one lower than the highest, i.e. priority 3 (configMAX_PRIORITIES - 1),

The two functions TaskRedBlinky() and TaskYellowBlinky() are identical, apart from the fact that they read in different potentiometers or control different LEDs. In task functions vTaskDelay() must be used instead of the normal delay() function. vTaskDelay() ensures that a resheduling takes place. Please note that vTaskDelay() does not interpret the passed value as milliseconds but as number of ticks (the constant portTICK_PERIOD_MS corresponds to the number of ms per tick = 16ms/tick).

Source code: https://github.com/schaeren/micro_controller/tree/master/arduino_avr/c/arduino_avr_rtosTask01 (for Arduino Uno and Arduino Mega 2560 Rev 3):

#include <Arduino.h>
#include <Arduino_FreeRTOS.h>

uint8_t redLedPin = 4;      // output for red LED
uint8_t yellowLedPin = 3;   // output for yellow LED
uint8_t redPotPin = A0;     // ADC input for potentiometer for red LED
uint8_t yellowPotPin = A1;  // ADC input for potentiometer for yellow LED

void TaskRedBlinky([[maybe_unused]] void *pvParameters )
{
    pinMode(redLedPin, OUTPUT);
    pinMode(redPotPin, INPUT);

    for (;;) {
        digitalWrite(redLedPin, !digitalRead(redLedPin));
        long potValue = analogRead(redPotPin);
        vTaskDelay((25 + potValue) / portTICK_PERIOD_MS);  // min. delay = 25ms
    }
}

void TaskYellowBlinky([[maybe_unused]] void *pvParameters )
{
    pinMode(yellowLedPin, OUTPUT);
    pinMode(yellowPotPin, INPUT);

    for (;;) {
        digitalWrite(yellowLedPin, !digitalRead(yellowLedPin));
        long potValue = analogRead(yellowPotPin);
        vTaskDelay((25 + potValue) / portTICK_PERIOD_MS);  // min. delay = 25ms
    }
}

void setup() {
    Serial.begin(115200);
    Serial.print("configMAX_PRIORITIES = ");
    Serial.println(configMAX_PRIORITIES);
    UBaseType_t taskPriority = configMAX_PRIORITIES - 1 ;

    xTaskCreate(
        TaskRedBlinky,      // task function
        "TaskRedBlinky",    // task name
        100,                // stack size (bytes)
        NULL,               // paramater for task
        taskPriority,       // task priority
        NULL );             // task handle

    xTaskCreate(
        TaskYellowBlinky,   // task function
        "TaskYellowBlinky", // task name
        100,                // stack size (bytes)
        NULL,               // paramater for task
        taskPriority,       // task priority
        NULL );             // task handle
}

void loop() {
    // idle task
}

Remark:

• The project (https://github.com/schaeren/micro_controller/tree/master/arduino_avr/c/arduino_avr_rtosTask01) supports both, Arduino Uno and Arduino Mega 2560 Rev 3. Before compiling or downloading you have to select the target system in platformio.ini:
  [platformio]
default_envs = uno ; 'uno' or 'megaatmega2560'

Parametrized tasks

In the previous example we used two identical function TaskRedBlinky() and TaskYellowBlinky() for the two tasks. The next example simplifies this by using only one task function, but with parameters: TaskBlinky(). The task parameters are contained in two structs taskRedBlinkyParams and taskYelloBlinkyParams.

Source code: https://github.com/schaeren/micro_controller/tree/master/arduino_avr/c/arduino_avr_rtosTask02

#include <Arduino.h>
#include <Arduino_FreeRTOS.h>

uint8_t redLedPin = 4;      // output for red LED
uint8_t yellowLedPin = 3;   // output for yellow LED
uint8_t redPotPin = A0;     // ADC input for potentiometer for red LED
uint8_t yellowPotPin = A1;  // ADC input for potentiometer for yellow LED

// struct used for task parameters
struct TaskParams_t {
    uint8_t ledPin;
    uint8_t potPin;
};

// Prepare task parameters
TaskParams_t taskRedBlinkyParams = {
    .ledPin = redLedPin, 
    .potPin = redPotPin
};
TaskParams_t taskYellowBlinkyParams = {
    .ledPin = yellowLedPin, 
    .potPin = yellowPotPin
};

void TaskBlinky([[maybe_unused]] void *pvParameters )
{
    TaskParams_t* param = (TaskParams_t *) pvParameters;
    uint8_t ledPin = param->ledPin;
    uint8_t potPin = param->potPin;
    pinMode(ledPin, OUTPUT);
    pinMode(potPin, INPUT);

    for (;;) {
        digitalWrite(ledPin, !digitalRead(ledPin));
        long potValue = analogRead(potPin);
        vTaskDelay((25 + potValue) / portTICK_PERIOD_MS);  // min. delay = 25ms
    }
}

void setup() {
    UBaseType_t taskPriority = configMAX_PRIORITIES - 1 ;

    xTaskCreate(
        TaskBlinky,             // task function
        "TaskBlinky-red",       // task name
        100,                    // stack size (bytes)
        &taskRedBlinkyParams,   // paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskBlinky,             // task function
        "TaskBlinky-yellow",    // task name
        100,                    // stack size (bytes)
        &taskYellowBlinkyParams,// paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle
}

void loop() {
    // idle task
}

Mutex (binary semaphore)

In freeRTOS mutex and binary semaphores are similar synchronization elements, but unlike binary semaphores however – mutexes employ priority inheritance. This means that if a high priority task blocks while attempting to obtain a mutex (token) that is currently held by a lower priority task, then the priority of the task holding the token is temporarily raised to that of the blocking task. This mechanism is designed to ensure the higher priority task is kept in the blocked state for the shortest time possible, and in so doing minimise the ‘priority inversion’ that has already occurred.

The following example is derived from the previous one, but now reading the analog-digital converter (ADC) and controlling the LED are divided into different tasks. The voltage values are read from ADC are stored in common variables. The LED tasks read these values and control the LEDs. The access to the variables is controlled by mutex.

Note: This is rather an untypical example for the use of mutex, because one task only writes to the variables and the other ones only read the variables, which is rather uncritical. More critical would be e.g. if several tasks read a variable and change it depending on its value, because then between reading and changing another task could also change the same variable, which could possibly lead to a wrong double change. But here we just want to show how to use mutex.

Create mutex:

SemaphoreHandle_t delaysMutex;

delaysMutex = xSemaphoreCreateMutex();

The mutex is used to protect access to variables (the normal semaphore functions are used to access the mutex, xSemaphoreTake() and xSemaphoreGive()). The second parameter of xSemaphoreTake() defines the maximum time to wait for the release of the mutex (in ticks), portMAX_DELAY waits infinitely (may depend on freeRTOS configuration).

if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
    ...
    xSemaphoreGive(delaysMutex);
}

The function TaskAnalogIn() implements the task which reads the voltage values from the ADC and stores them in the variables delayRed and delayYellow. The short waiting time (delayBeforeAdcRead = 1 tick = 16 ms) before reading from the ADC can improve the accuracy of the ADC value, because the input value can stabilize after switching the analog input channel. The values read from the 10-Bit ADC in the range 0…1023 are normalized to the range 16…1000 that will be used as delay times for the LEDs, where the minimum time is 16 ms and the maximum is 1000 ms. 16ms ensures that the flashing still remains barely visible (1 / (2 * 16 ms) = 31.25 Hz.

void TaskAnalogIn([[maybe_unused]] void *pvParameters )
{
    pinMode(redPotPin, INPUT);
    pinMode(yellowPotPin, INPUT);

    // Temporary local variables for delay times [ms], at the end of a loop 
    // these values will be written to global variables, too.
    int16_t _delayRed = 0, _delayYellow = 0;

    for (;;) {
        // Get delay for red LED by reading voltage potentiometer 'red'
        vTaskDelay(delayBeforeAdcRead);                             // Wait a short time to stabilize the ADC value
        int potRedValue = analogRead(redPotPin);                    // Read value from ADC
        _delayRed = map(potRedValue, 0, 1023, minDelay, maxDelay);  // normalize ADC value to 16...1000
        
        // Get delay for yellow LED by reading voltage potentiometer 'yellow'
        vTaskDelay(delayBeforeAdcRead);                             // Wait a short time to stabilize the ADC value
        int potYellowValue = analogRead(yellowPotPin);              // Read value from ADC
        _delayYellow = map(potYellowValue, 0, 1023, minDelay, maxDelay); // normalize ADC value to 16...1000

        // Write normalized values to global variables, access is protected by mutex 
        // (to be used as delay times [ms])
        if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
            delayRed = _delayRed;
            delayYellow = _delayYellow;
            xSemaphoreGive(delaysMutex);
        }
    }
}

The function TaskBlinky() is started as task for each LED. It gets the delay time from the global variables delayRed and delayYellow.

void TaskBlinky([[maybe_unused]] void *pvParameters )
{
    TaskParams_t* param = (TaskParams_t *) pvParameters;
    uint8_t ledPin = param->ledPin;

    pinMode(ledPin, OUTPUT);

    for (;;) {
        // Get delay time [ms] from global varibale, access is protected by mutex.
        int16_t _delayTime;
        if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
            _delayTime = *(param->delayTime);
            xSemaphoreGive(delaysMutex);
        }
        if (_delayTime != -1) {
            digitalWrite(ledPin, !digitalRead(ledPin));
            long _delayTicks = _delayTime / portTICK_PERIOD_MS;
            vTaskDelay(_delayTicks);
        }
    }
}

The complete source code can be found here: https://github.com/schaeren/micro_controller/blob/master/arduino_avr/c/arduino_avr_rtosMutex/src/main.cpp

Message queue

Queues allow to send messages between tasks, they can be used as first in / first out (FIFO) or last in / first out (LIFO) queues. A message can be any data (byte array) of a fixed size.

Um die Verwendung einer Queue zu zeigen, wird das bestehende Beispiel erweitert: Ein zusätzlicher Task soll Meldungen aus, welche die aktuellen Delay-Werte enthalten, aus einer Queue lesen und diese über die Serial Konsole ausgeben. Der bestehende Task TaskAnalogIn() schreib die aktuellen Werte von delayRed und delayYellow in die Queue, sobald sich diese ändern.

To demonstrate the use of a queue, the previous example is extended: An additional task is to read messages with current delay times from a queue and output them to the serial console. The existing Task TaskAnalogIn() writes the current delay times of delayRed and delayYellow into the queue as soon as they change.

Before a queue can be used it must be created with xQueueCreate():

QueueHandle_t delaysQueue;
UBaseType_t delaysQueueLen = 3;
struct DelaysQueueEntry_t {
    int16_t delayRed;
    int16_t delayYellow;
};

...

delaysQueue = xQueueCreate(delaysQueueLen, sizeof(DelaysQueueEntry_t));
if (delaysQueue == NULL) {
    Serial.println("Failed to create queue 'delaysQueue'!");
}

With the function xQueueSendToBack() a message can be sent into a queue’s end (LIFO queue). The third parameter (here portMAX_DELAY) allows to specify the maximum time to wait if the queue is already full. The message is physically copied into the queue, so the message variable may be overwritten/reused after sending to queue:

DelaysQueueEntry_t message = {
    .delayRed = ...,
    .delayYellow = ...
};
if (xQueueSendToBack(delaysQueue, (void*) &message, portMAX_DELAY) != pdPASS) {
    Serial.println("Failed to send message to queue 'delaysQueue'!");
}

Finally the function xQueueReceive() allows to receive messages from a queue’s end. Again the third parameter (here portMAX_DELAY) allows to specify the maximum time to wait if the queue is empty.

DelaysQueueEntry_t message;
if (xQueueReceive(delaysQueue, (void*) &message, portMAX_DELAY) == pdTRUE) {
    ...
} else {
    Serial.println("Task TaskLogWriter: Failed to receive message from queue 'delaysQueue'!");
}

This is the modified function TaskAnalogIn() which uses the queue:

void TaskAnalogIn([[maybe_unused]] void *pvParameters )
{
    int16_t _oldDelayRed = 0;
    int16_t _oldDelayYellow = 0;

    pinMode(redPotPin, INPUT);
    pinMode(yellowPotPin, INPUT);

    // Temporary local variables for delay times [ms], at the end of a loop 
    // these values will be written to global variables, too.
    int16_t _delayRed = 0, _delayYellow = 0;

    for (;;) {
        // Get delay for red LED
        vTaskDelay(delayBeforeAdcRead);
        int potRedValue = analogRead(redPotPin);
        _delayRed = map(potRedValue, 0, 1023, minDelay, maxDelay);

        // Get delay for yellow LED
        vTaskDelay(delayBeforeAdcRead);
        int potYellowValue = analogRead(yellowPotPin);
        _delayYellow = map(potYellowValue, 0, 1023, minDelay, maxDelay);

        if (abs(_delayRed - _oldDelayRed) > minDelayDiff || abs(_delayYellow - _oldDelayYellow) > minDelayDiff) {
            _oldDelayRed = _delayRed;
            _oldDelayYellow = _delayYellow;
            // Write delay times [ms] to global variables, access is protected by mutex.
            if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
                delayRed = _delayRed;
                delayYellow = _delayYellow;
                xSemaphoreGive(delaysMutex);
            }
            DelaysQueueEntry_t message = {
                .delayRed = _delayRed,
                .delayYellow = _delayYellow
            };
            if (xQueueSendToBack(delaysQueue, (void*) &message, portMAX_DELAY) != pdPASS) {
                Serial.println("Failed to send message to queue 'delaysQueue'!");
            }
        }
    }
}

This is the new function/task TaskLogWriter() which reads from the queue and writes to Serial:

void TaskLogWriter([[maybe_unused]] void *pvParameters )
{
    DelaysQueueEntry_t message;
    for (;;) {
        if (xQueueReceive(delaysQueue, (void*) &message, portMAX_DELAY) == pdTRUE) {
            char text[50];
            sprintf(text, "red: %-4d ms, yellow: %-4d ms.", message.delayRed, message.delayYellow);
            Serial.println(text);
        } else {
            Serial.println("Task TaskLogWriter: Failed to receive message from queue 'delaysQueue'!");
        }
    }
}

The complete source code can be found here: https://github.com/schaeren/micro_controller/blob/master/arduino_avr/c/arduino_avr_rtosQueue/src/main.cpp

Advanced example

The last example is a greatly extended version of the last example program.

IMPORTANT: Due to the various extensions, this example needs more than 2Kbyte of RAM and therefore no longer runs on the board Arduino Uno! But it runs fine on Arduino Mega 2560 wit 8Kbytes of RAM.

Some of the enhancements compared to the last version are:

• Another task function TaskDigitalInput() controls a third LED (green) whose blinking frequency can be changed by means of three push buttons (‘stop’, ‘slow’, ‘fast’). This task does not read the buttons itself, but reads the last pressed button from a second queue greenQueue and determines the delay for the green LED (‘stop’ = -1, ‘slow’ = 500 ms, ‘fast’ = 50 ms).
• An interrupt handler (ISR(PCINT0_vect)), which is triggered via ‘Pin Change Interrupt‘ when one of the three push buttons is hit, sends a corresponding message (‘stop’, ‘slow’, ‘fast’) to the mentioned greenQueue.
• Another task function TaskLcd() controls a small LCD display. On this display the current delay times of the three LEDs are shown.
• The task function TaskLogWriter() and its queue logQueue accepts string messages. So from any task formatted string messages can be written at any time to the Serial output using the helper method sendTextToLogQueue().
• The two potentiometers for the red and yellow LEDs are supplied with 3.3V. Since this voltage is stabilized on the Arduino board itself, the voltage noise is a bit less than the 5V which is fed in via the USB cable. Thus also the values measured with the ADC are possibly somewhat more stable.
• Near the top of the main.cpp several callback functions are implemented which are called from the freeRTOS runtime. So, e.g. all heap memory allocation and deallocations (by pvPortMalloc() and vPortFree()) are logged to the Serial output. This gives an idea about how much heap memory is used.

Tasks and interrupt handlers:

• TaskAnalogInput
  Reads two ADC inputs and writes these values to the global variables delayRed and delayYellow.
• TaskDigitalInput
  Reads messages from greenQueue, i.e. push buttons presses (‘stop’, ‘slow’, ‘fast’) and writes delay time to global variable delayGreen.
• TaskBlinky (started 3 times)
  Switches on and off a LED with frequency according to delayRed or delayYellow or delayGreen, depending on task instance.
• TaskLcd
  Displays the current values of delayRed, delayYellow, delayGreen
• TaskLogWriter
  Write text messages received from all tasks to Serial output. Any task may write text messages at any time to the logQueue.
• ISR(PCINT0_vect) Interrupt handler
  Called when a push button is pressed. Writes messages to greenQueue.

Queues:

• greenQueue
  Each button press creates a message which is sent to this queue by interrupt handler ISR(PCINT0_vect).
  Task TaskAnalogInput receives these messages.
• logQueue
  Any task may write a text message at any time to this queue.
  Task TaskLogWriter receives this messages.

Mutex:

• delaysMutex
  Used to protect the global variables delayRed, delayYellow and delayGreen.

Source code: https://github.com/schaeren/micro_controller/tree/master/arduino_avr/c/arduino_avr_rtosMega

#define DEBUG

#include <Arduino.h>
#include <Arduino_FreeRTOS.h>
#include <LiquidCrystal.h>
#include <semphr.h>
#include <queue.h>
#include <portable.h>

// Stack sizes for tasks (experimentally determined approximately, multiplied by 2)
const configSTACK_DEPTH_TYPE stackTaskBlinky = 70 * 2;
const configSTACK_DEPTH_TYPE stackTaskAnalogInput = 80 * 2;
const configSTACK_DEPTH_TYPE stackTaskDigitalInput = 80 * 2;
const configSTACK_DEPTH_TYPE stackTaskLogWriter = 110 * 2;
const configSTACK_DEPTH_TYPE stackTaskLcd = 100 * 2;

long __heapSize = 0;
void myTraceMalloc(size_t size, void* ptr)
{
#ifdef DEBUG
    __heapSize += size;
    Serial.print("pvPortMalloc() size=");
    Serial.print(size);
    Serial.print(", total allocated bytes=");
    Serial.print(__heapSize);
    Serial.print(", ptr=");
    Serial.println((long)ptr);
#endif
}
void myTraceFree(void* ptr)
{
#ifdef DEBUG
    Serial.print("vPortFree()    ptr=");
    Serial.println((long)ptr);
#endif
}
void vApplicationMallocFailedHook( void )
{
    Serial.println("*** MALLOC FAILED ***");
}
#undef configCHECK_FOR_STACK_OVERFLOW
#define configCHECK_FOR_STACK_OVERFLOW = 2
void vApplicationStackOverflowHook( TaskHandle_t xTask, signed char *pcTaskName )
{
    // Seems not top work with Arduino (?)
    Serial.print("*** STACK OVERFLOW IN TASK ");
    Serial.print((char*)pcTaskName);
    Serial.println(" ***");
}

uint8_t redLedPin = 4;          // Output for red LED
uint8_t yellowLedPin = 3;       // Output for yellow LED
uint8_t greenLedPin = 2;        // Output for green LED
uint8_t redPotPin = A0;         // ADC input for potentiometer for red LED
uint8_t yellowPotPin = A1;      // ADC input for potentiometer for yellow LED
uint8_t voltageRefPin = A2;     // ADC input connected to 3.3V reference
uint8_t stopButtonPin = 10;     // Input for button 'stop stop' of green LED
uint8_t slowButtonPin = 11;     // Input for button 'slow blicking' of green LED
uint8_t fastButtonPin = 12;     // Input for button 'fast blinking' of green LED

// Configuration
const long delayBeforeAdcRead = 2; // Wait time before ADC is read in ticks (1 tick = 15ms)
                                   // Remark: A short recovery time between the reading of
                                   // different ADC channels leads to more stable results.
const long minDelay = portTICK_PERIOD_MS; // Min. delay = min. half period time for LED flashing in ms
const long maxDelay = 1000;     // Max. delay = max. half period time for LED flashing in ms
const long minDelayDiff = 5;    // Min. difference so that delay change is written logQueue in ms
const long slowDelay = 500;     // Delay for slow blinking of green LED (1 Hz)
const long fastDelay = 50;      // Delay for faslt blinking of green LED (10 Hz)

// Half period time for LEDs, access is protected by mutex
SemaphoreHandle_t delaysMutex;
int16_t delayRed = -1;          // ms
int16_t delayYellow = -1;       // ms
int16_t delayGreen = -1;        // ms

// Messag queue for log writer
QueueHandle_t logQueue;
UBaseType_t logQueueLen = 3;
typedef char LogQueueEntry_t[50];

// Messag queue used to send messages for interrupt handler to TaskDigitalInput
QueueHandle_t greenQueue;
UBaseType_t greenQueueLen = 2;
enum GreenQueueEntry_t { stop, slow, fast, undefined };

// struct used for task parameters
struct TaskParams_t {
    const char* color;
    uint8_t ledPin;
    int16_t* delayTime;
};

TaskParams_t taskRedBlinkyParams = {
    .color = "red",
    .ledPin = redLedPin,
    .delayTime = &delayRed
};
TaskParams_t taskYellowBlinkyParams = {
    .color = "yellow",
    .ledPin = yellowLedPin,
    .delayTime = &delayYellow
};
TaskParams_t taskGreenBlinkyParams = {
    .color = "green",
    .ledPin = greenLedPin,
    .delayTime = &delayGreen
};

void sendTextToLogQueue (const char* format, ...);

void TaskAnalogInput([[maybe_unused]] void *pvParameters )
{
    int16_t maxAdcValue = 0;    // ADC readout value for 3.3V
    int16_t _oldDelayRed = -1;
    int16_t _oldDelayYellow = -1;

    sendTextToLogQueue("Task AnalogIn started, input pins: %d, %d", redLedPin, yellowLedPin);
    pinMode(redPotPin, INPUT);
    pinMode(yellowPotPin, INPUT);
    maxAdcValue = analogRead(voltageRefPin);

    // Temporary local variables for delay times [ms], at the end of a loop
    // these values will be written to global variables, too.
    int16_t _delayRed, _delayYellow = 0;

    for (;;) {
        // Get delay for red LED
        vTaskDelay(delayBeforeAdcRead);
        int potRedValue = analogRead(redPotPin);
        _delayRed = map(potRedValue, 0, maxAdcValue, minDelay, maxDelay);

        // Get delay for yellow LED
        vTaskDelay(delayBeforeAdcRead);
        int potYellowValue = analogRead(yellowPotPin);
        _delayYellow = map(potYellowValue, 0, maxAdcValue, minDelay, maxDelay);

        // If at least one delay time [ms] has changed by more than minDelayDiff milliseconds,
        // write it to global variables, access is protected by mutex.
        if (abs(_delayRed - _oldDelayRed) > minDelayDiff || abs(_delayYellow - _oldDelayYellow) > minDelayDiff) {
            _oldDelayRed = _delayRed;
            _oldDelayYellow = _delayYellow;
            if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
                delayRed = _delayRed;
                delayYellow = _delayYellow;
                xSemaphoreGive(delaysMutex);
            }
            sendTextToLogQueue("red: %dms, yellow: %dms.", _delayRed, _delayYellow);
        }
    }
}

void TaskDigitalInput([[maybe_unused]] void *pvParameters )
{
    int16_t _oldDelayGreen = -1;

    sendTextToLogQueue("Task DigitalIn started, input pins: %d, %d, %d",
        stopButtonPin, slowButtonPin, fastButtonPin);
    
    for (;;) {
        GreenQueueEntry_t cmd;
        if (xQueueReceive(greenQueue, &cmd, portMAX_DELAY) != pdTRUE) {
            Serial.println("TaskDigitalInput");
        }
        int16_t _delayGreen = -1;
        switch (cmd) {
            case stop:
                _delayGreen = -1;
                break;
            case slow:
                _delayGreen = slowDelay;
                break;
            case fast:
                _delayGreen = fastDelay;
                break;
            default:
                break;
        }
        // If delay time [ms] has changed, write it to global variables,
        // access is protected by mutex.
        if (_delayGreen != _oldDelayGreen) {
            _oldDelayGreen = _delayGreen;
            if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
                delayGreen = _delayGreen;
                xSemaphoreGive(delaysMutex);
            }
            sendTextToLogQueue("green: %dms.", _delayGreen);
        }
        vTaskDelay(5);  // wait 75 ms
    }
}

void TaskBlinky([[maybe_unused]] void *pvParameters )
{
    TaskParams_t* param = (TaskParams_t *) pvParameters;
    const char* color = param->color;
    uint8_t ledPin = param->ledPin;
    sendTextToLogQueue("Task Blinky for %s started, output pin %d", color, ledPin);

    pinMode(ledPin, OUTPUT);

    for (;;) {
        // Get delay time [ms] from global varibale, access is protected by mutex.
        int16_t _delayTime;
        if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
            _delayTime = *(param->delayTime);
            xSemaphoreGive(delaysMutex);
        }

        if (_delayTime != -1) {
            digitalWrite(ledPin, !digitalRead(ledPin));
            long _delayTicks = round((0.0 + _delayTime) / portTICK_PERIOD_MS);
            vTaskDelay(_delayTicks);
        } else {
            digitalWrite(ledPin, LOW);
            vTaskDelay(1); // 16 ms
        }
    }
}

void TaskLogWriter([[maybe_unused]] void *pvParameters )
{
    LogQueueEntry_t buffer;

    for (;;) {
        if (xQueueReceive(logQueue, buffer, portMAX_DELAY) == pdTRUE) {
            Serial.println(buffer);
        } else {
            Serial.println("Task TaskLogWriter: Failed to receive from queue.");
        }
    }
}

// Mapping of LCD pins to Arduino Mega pins.
const int rs = 50, en = 48, d4 = 46, d5 = 44, d6 = 42, d7 = 40;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

void TaskLcd([[maybe_unused]] void *pvParameters)
{
    int16_t _delayRed, _delayYellow,_delayGreen = 0;
    int16_t _lastDelayRed, _lastDelayYellow,_lastDelayGreen = 0;
    sendTextToLogQueue("Task TaskLcd started");

    // Set up the LCD's number of columns and rows.
    lcd.begin(16, 2);
    // Print a message to the LCD.
    lcd.print("Starting...");
    vTaskDelay(1000 / portTICK_PERIOD_MS); // 1 s

    for (;;) {
        if (xSemaphoreTake(delaysMutex, portMAX_DELAY) == pdTRUE) {
            _delayRed = delayRed;
            _delayYellow = delayYellow;
            _delayGreen = delayGreen;
            xSemaphoreGive(delaysMutex);
        }
        if (_delayRed != _lastDelayRed || _delayYellow != _lastDelayYellow || _delayGreen != _lastDelayGreen) {
            _lastDelayRed = _delayRed;
            _lastDelayYellow = _delayYellow;
            _lastDelayGreen = _delayGreen;
            char text[17];
            sprintf(text, "r=%d, y=%d", _delayRed, _delayYellow);
            lcd.clear();
            lcd.print(text);
            lcd.setCursor(0, 1);
            sprintf(text, "g=%d", _delayGreen);
            lcd.print(text);
        }
        vTaskDelay(5); // 80 ms
    }
}

// Interrupt handler for 'Port Change Interrupt' inputs 10..12
ISR(PCINT0_vect)
{
    static long lastUpdateTime = 0;
    const long debounceDuration = 20; // ms
    static long lastStopButtonState = HIGH;
    static long lastSlowButtonState = HIGH;
    static long lastFastButtonState = HIGH;

    long stopButtonState = digitalRead(stopButtonPin);
    long slowButtonState = digitalRead(slowButtonPin);
    long fastButtonState = digitalRead(fastButtonPin);

    long now = millis();
    if ((stopButtonState != lastStopButtonState ||
         slowButtonState != lastSlowButtonState ||
         fastButtonState != lastFastButtonState) && now - debounceDuration > lastUpdateTime) {
        lastStopButtonState = stopButtonState;
        lastSlowButtonState = slowButtonState;
        lastFastButtonState = fastButtonState;
        lastUpdateTime = now;

        GreenQueueEntry_t cmd = undefined;
        if (stopButtonState == LOW) {
            cmd = stop;
        } else if (slowButtonState == LOW) {
            cmd = slow;
        } else if (fastButtonState == LOW) {
            cmd = fast;
        }
        if (cmd != undefined) {
            BaseType_t higherPriorityTaskWoken;
            if (xQueueSendToFrontFromISR(greenQueue, &cmd, &higherPriorityTaskWoken) != pdTRUE) {
                Serial.println("ISR: Failed to send command to queue 'greenQueue'.");
            }
        }
    }
}

void setup() {
    Serial.begin(115200);
    while (!Serial);

    Serial.print("configMAX_PRIORITIES=");
    Serial.println(configMAX_PRIORITIES);
    UBaseType_t taskPriority = configMAX_PRIORITIES - 1 ;

    // Create/initialize mutex (semaphore).
    delaysMutex = xSemaphoreCreateMutex();

    // Create queues
    logQueue = xQueueCreate(logQueueLen, sizeof(LogQueueEntry_t));
    if (logQueue == NULL) {
        Serial.println("Failed to create queue 'logQueueLen'.");
    }
    greenQueue = xQueueCreate(greenQueueLen, sizeof(GreenQueueEntry_t));
    if (greenQueue == NULL) {
        Serial.println("Failed to create queue 'greenQueue'.");
    }

    pinMode(stopButtonPin, INPUT_PULLUP);
    pinMode(slowButtonPin, INPUT_PULLUP);
    pinMode(fastButtonPin, INPUT_PULLUP);

    // Enable 'Pin Change Interrupts' for inputs 10..12
    noInterrupts();
    // PCICR – Pin Change Interrupt Control Register
    PCICR |= 1 << PCIE0; // Enable pin change irq from port B (i.e. Arduino i/o 10..13)
    // PCMSK0 - Pin Change Mask Register 0
    PCMSK0 |= 1 << PCINT4 | 1 << PCINT5 | 1 << PCINT6; // Enable PCINT6..6 (i.e. Arduino i/o 10..12)
    interrupts();

    // Create/start tasks.
    xTaskCreate(
        TaskLogWriter,          // task function
        "TaskLogWriter",        // task name
        stackTaskLogWriter,     // stack size
        NULL,                   // paramater for task
        taskPriority + 1,       // task priority (higher than other tasks!)
        NULL );                 // task handle

    xTaskCreate(
        TaskAnalogInput,        // task function
        "TaskAnalogInput",      // task name
        stackTaskAnalogInput,   // stack size
        NULL,                   // paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskDigitalInput,       // task function
        "TaskDigitalInput",     // task name
        stackTaskDigitalInput,  // stack size
        NULL,                   // paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskBlinky,             // task function
        "TaskBlinky-red",       // task name
        stackTaskBlinky,        // stack size
        &taskRedBlinkyParams,   // paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskBlinky,             // task function
        "TaskBlinky-yellow",    // task name
        stackTaskBlinky,        // stack size
        &taskYellowBlinkyParams,// paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskBlinky,             // task function
        "TaskBlinky-green",     // task name
        stackTaskBlinky,        // stack size
        &taskGreenBlinkyParams, // paramater for task
        taskPriority,           // task priority
        NULL );                 // task handle

    xTaskCreate(
        TaskLcd,                // task function
        "TaskLcd",              // task name
        stackTaskLcd,           // stack size
        NULL,                   // paramater for task
        taskPriority - 1,       // task priority
        NULL );                 // task handle

     vTaskStartScheduler();
     Serial.println("Failed to start RTOS scheduler!");
}

void loop() {
    // idle task
}

void sendTextToLogQueue (const char* format, ...) {
    va_list argptr;
    va_start(argptr, format);
    LogQueueEntry_t sendBuffer;
    vsprintf(sendBuffer, format, argptr);
    va_end(argptr);
    if (xQueueSend(logQueue, sendBuffer, portMAX_DELAY) != pdPASS) {
        pinMode(greenLedPin, OUTPUT);
        digitalWrite(greenLedPin, HIGH);
        Serial.println("sendTextToLogQueue() failed!");
    }
}