スレッド間同期した一定時間間隔スレッドの作成

一定時間間隔の制御と制御同期入出力をFreeRTOSで実装したのでまとめ

やったこと

  • 一定時間間隔の入出力スレッド(IoThread)
  • IoThreadと制御スレッド(ControlThread)のセマフォ同期
  • ミューテックス排他制御によるIoThreadとControlThreadのリソース管理
  • キューによるControlThreadとPC通信スレッド(MainteThread)間通信

想定読者

  • (Free)RTOSに興味がある人
  • 組み込みで一定時間間隔制御が必要な人

シーケンス図，レポジトリ

f:id:Libra23:20220101195018p:plain
github.com

一定時間間隔スレッド生成

スレッド周期制御クラス
ThreadClock::ThreadClock(uint32_t ms) {
    previous_wake_time_ = xTaskGetTickCount();
    wait_ms_ = ms;
}

void ThreadClock::Wait() {
    vTaskDelayUntil(&previous_wake_time_, wait_ms_ / portTICK_RATE_MS);
}

void ThreadClock::Reset() {
    previous_wake_time_ = xTaskGetTickCount();
}

vTaskDelayUntil はタスクの実行時間が第一引数(previous_wake_time_)から
第二引数(wait_ms_ / portTICK_RATE_MS)だけ追加した時間になるまで待機する
第一引数はアドレス渡ししており，関数実行時に更新される

スレッド周期制御クラスを利用した一定時間間隔スレッド
void IoInterface::Thread() {
    // initialize
    clock_.Reset();
    // main loop
    while(true) {
        // cycle period
        clock_.Wait();
        // main
        Excute();
        // synchronize with semaphore
        if (counter_ % static_cast<int>(ROBOT_CONTROL_CYCLE_TIME_MS / IO_INTERFACE_CYCLE_TIME_MS) == 0) {
            sync_semaphore_.Give();
        }
        // update
        counter_++;
    }
}

clock_ が先ほどのTreadClockクラスのオブジェクト
スレッド開始時にReset()を行い，ループごとにWait()して一定時間間隔を実現

スレッド周期決め

IoThreadでは12個のシリアルサーボに1250000 bpsでUART通信している
1サーボあたり出力は3 byte(= 24 bit)なので5 msとした
（cycle_time_ms = 24 * 12 / 1250000 = 2.3 [ms] >> 5 [ms]）

スレッド間のセマフォ同期

Semaphore::Semaphore() {
    x_semaphore_ = xSemaphoreCreateBinary();
}

bool Semaphore::Take() {
    if (xSemaphoreTake(x_semaphore_, portMAX_DELAY) == pdTRUE) {
        return true;
    } else {
        return false;
    }
}

bool Semaphore::Give() {
    if (xSemaphoreGive(x_semaphore_) == pdTRUE) {
        return true;
    } else {
        return false;
    }
}
  • xSemaphoreCreateBinary() でバイナリセマフォ作成
  • xSemaphoreTake(x_semaphore_, portMAX_DELAY) でセマフォ許可待ち
  • xSemaphoreGive(x_semaphore_) でセマフォ許可通知

バイナリセマフォは許可通知を1つだけ発行できるセマフォで
１つのスレッドが許可通知をし，一方のスレッドが許可受信するまでスレッドを待機させることができる
xSemaphoreGive()が呼ばれた時にxSemaphoreTake()の待機が完了し，
再度xSemaphoreGive()が呼ばれるまで待機する

セマフォ許可通知するIoThread側
void IoInterface::Thread() {
    // initialize
    clock_.Reset();
    // main loop
    while(true) {
        // cycle period
        clock_.Wait();
        // main
        Excute();
        // synchronize with semaphore
        if (counter_ % static_cast<int>(ROBOT_CONTROL_CYCLE_TIME_MS / IO_INTERFACE_CYCLE_TIME_MS) == 0) {
            sync_semaphore_.Give();
        }
        // update
        counter_++;
    }
}

sync_semaphore_ が先ほどのSemaphoreクラスのオブジェクト
sync_semaphore_.Give() で周期的にセマフォ許可を通知している

セマフォ許可待ちするControlThread側
void Robot::Thread() {
    // main loop
    while(true) {
        // synchronize with semaphore
        if(!sync_semaphore_.Take()) {
            break;
        }
        // main
        Excute();
        // update
        counter_++;
    }
    vTaskDelete(NULL);
}

sync_semaphore_.Take() でセマフォ許可待ちしている
セマフォが取得できなかった場合はスレッド終了

スレッド周期決め

IoThreadでセマフォ許可通知を2回に1回にし，10 msとした
（cycle_time_ms = 5 * 2 = 10 [ms]）

ミューテックス排他制御

template <typename T>
ShareMemory<T>::ShareMemory() {
    x_mutex_ = xSemaphoreCreateMutex();
    memory_ = T();
}

template <typename T>
bool ShareMemory<T>::Write(const T& data) {
    if (xSemaphoreTake(x_mutex_, portMAX_DELAY) == pdTRUE) {
        memory_ = data;
        xSemaphoreGive(x_mutex_);
        return true;
    } else {
        return false;
    }
}

template <typename T>
bool ShareMemory<T>::Read(T& data) {
    if (xSemaphoreTake(x_mutex_, portMAX_DELAY) == pdTRUE) {
        data = memory_;
        xSemaphoreGive(x_mutex_);
        return true;
    } else {
        return false;
    }
}

xSemaphoreCreateMutex() でミューテックス作成
セマフォは他のスレッドにセマフォ取得許可をするが，
ミューテックスは他のスレッドのミューテックス取得を禁止する
同じx_mutex_においてxSemaphoreTake() を呼んだ後は
xSemaphoreGive() が呼ばれるまでxSemaphoreTake() を呼んでも待機したままになる

キューによるスレッド間通信

Queue::Queue(uint32_t length, uint32_t size) {
    x_queue_ = xQueueCreate(length, size);
}

bool Queue::Send(const void* data) {
    if (xQueueSendToBack(x_queue_, data, portMAX_DELAY) == pdTRUE) {
        return true;
    } else {
        return false;
    }
}

bool Queue::Receive(void* data) {
    if (xQueueReceive(x_queue_, data, portMAX_DELAY) == pdTRUE) {
        return true;
    } else {
        return false;
    }
}
  • xQueueCreate() でキュー作成
  • xQueueSendToBack() でキューにデータを配置
  • xQueueReceive() でキューからデータを取得
キュー送信するMainteThread側
MsgCmdControl cmd(MsgType::MSG_MAINTE_TO_ROBOT_CONTROL_ON, packet.arm_id, packet.control_data);
mainte_to_robot_queue_.Send(&cmd);

mainte_to_robot_queue_ が先ほどのQueueクラスのオブジェクト
MsgCmdControl 構造体のデータを作成し，Send(&cmd)で送信している

キュー受信するControlThread側
uint8_t buf[GetMaxMsgSize()];
mainte_to_robot_queue_.Receive(buf);

Receive(buf)で受信