ARM CMSIS DSP_Lib PID

　CMSIS DSP_LibのPIDを試してみた。

　実際にそのままのコードを使うとワインドアップ(wind-up)してしまうので、その対策とか。

　CMSIS DSP PIDは、入力値が0になるような操作を行う。そのため、入力値には目標値-現在値を与えることになる。

　上はPIDをそのまま使ったときのグラフ、下はワインドアップ対策を行ったときのグラフ。
　CMSISのPIDはよくあるPIDの解説とはちょっと違う感じの処理になっている気がする。いまいちパラメーターを煮詰めきれていない。
　傾向として、Kiが1を超えると発散し、Kiが1を下回れば収束する感じ。今回はKp = 10, Ki = 2, Kd = 0としている。
　もしかしたら、Kp, Ki, Kdは0-1の範囲で与える必要があるのかもしれない(q15やq31は0-1の範囲しか与えられない)。その場合は入力・出力も-1 - +1の範囲になるのかも。

　arm_pid_instanceのstate[]は、0が前回の入力値、1が前々回の入力値、2が前回の出力値、を保持している。
　ワインドアップは操作量が操作可能量を超えたときに発生し、操作量はstate[2]に保存されている。操作可能量を超えた場合は、state[2]にクランプ後の値を入れてやればいい。


　テストコード

arm_pid_instance_f32 pid = {};

pid.Kp = 10;
pid.Ki = 2;
pid.Kd = 0;

arm_pid_init_f32(&pid, 1);

const float32_t speed_limit = 100;
const float32_t acc_limit = 10;

float32_t position = 0;
float32_t target = 0;
float32_t speed = 0;

for (uint32_t i = 0; i < 10000; i++)
{
    switch (i)
    {
    case 1000:
        target = 10000;
        break;

    case 4000:
        target = -10000;
        break;

    case 8000:
        target = 0;
        break;
    }

    const float32_t prev_speed = speed;

    const float32_t diff = target - position;

    const float32_t tmp1 = arm_pid_f32(&pid, diff);
    const bool is_over_limit = check_over_limit(tmp1, -speed_limit, +speed_limit);

    const float32_t tmp2 = !is_over_limit ? tmp1 : clamp(tmp1, -speed_limit, +speed_limit);
    speed = clamp(tmp2, speed - acc_limit, speed + acc_limit);

    if (is_over_limit)
    {
        pid.state[2] = speed;
    }

    printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
            position, target, diff, speed, speed - prev_speed,
            pid.state[0], pid.state[1], pid.state[2]);

    position += speed;
}

***

　久しぶりにL6470で遊んでいる。
　L6470のSPIっていわゆるモード3なのね。モード0で負数を読んだときに変な値になってハマった。データシートのタイミングダイヤグラムを見るとたしかにモード3の形。

　L6470は複数のドライバを連携させて動作させられるので、そのあたりを作り込むのがなかなか大変。いまいち良い使い方がわからない。
　L6470のコマンドの発行とかを管理するクラスと、L6470の通信を管理するクラスに分けて、コマンドクラスはドライバ1個毎に紐づけ、通信クラスはそれらを管理、というふうにしたほうがいいのかな。
　いろいろ試行錯誤中。

　だいたい、この時期は天文関係のヤツに興味を持ち出す。去年の9月はアンドロメダを撮ってたし、11月は衛星追尾カメラを作ってた。もうちょっと暖かい時期にやってれば外での実験も楽でいいのにねぇ。
　ここ1年で自分のプログラミングスタイルもだいぶ変化したので、衛星追尾カメラを作り直し中。でもモータードライバで四苦八苦。