Ardupilot のAP_Mathクラスにある関数

はじめに

• 本記事は、Ardupilotに含まれている算術演算ライブラリ(AP_Math)に含まれている関数の作用についての覚書です。
• 関数の説明は随時更新したいと思います。

rotate関数

• rotate関数は__ジャイロセンサーの取付方向__ を__機体のRoll,Pitch,Yaw方向__ に合わせるために使用しているようです。
• Vector3型のメンバ変数__X,Y,Zの３つの入れ替え__ 処理を行います。
• 引数である enum Rotation により入れ替えのバリエーションを指定します。
• 関数は↓のように定義されています。

template <typename T>
void Vector3<T>::rotate(enum Rotation rotation){
   //処理記述(省略)
}

ローテーションなし（引数が__ROTATION_NONE__）の場合

• 機体ロール軸＝ジャイロセンサのX軸
• 機体ピッチ軸＝ジャイロセンサのY軸
• 機体ヨー軸＝ジャイロセンサのZ軸

となり、"X,Y,Zの入れ替えは無し" となります。

ヨー方向に90°（引数がROTATION_YAW_90）の場合

図のようにジャイロセンサを ヨー方向に90度回転して取り付けた状態だとします。

これを X=ロール、Y=ピッチ、Z=ヨー であるかのようにXYZを入れ変える処理が入ります。
**回転後X(機体ロール) ＝ ジャイロYの逆回転(符号が反転)
**回転後Y(機体ピッチ) ＝ ジャイロX
**回転後Z(機体ヨー ) ＝ ジャイロZ
となります。

該当部分のソースコードは↓の様に記述されています。

    switch (rotation) {
    //--省略--
    case ROTATION_YAW_90: {
        tmp = x;
        x = -y;
        y = tmp;
        return;
    }
    //--省略--

ロール90度回転後ピッチ90°度回転（引数がROTATION_ROLL_90_PITCH_90）の場合

先にソース参照します。 先にロール90度回転したのちにピッチ90度回転するような記述になっています

• 回転後Z(機体ヨー)＝ジャイロY
• 回転後Y(機体ピッチ)＝ジャイロZの逆回転

その後

• 回転後Z(機体ヨー)＝回転前Xの逆回転
• 回転後X(機体ロール)＝回転前Z

となります

    case ROTATION_ROLL_90_PITCH_90: {
        tmp = z;
        z = y;
        y = -tmp;
        tmp = z;
        z = -x;
        x = tmp;
        return;
    }

図で書くとこんな感じです。

• その他の回転も同じ要領でコーディングされていますので、説明は割愛します。