機体の初期姿勢が前方を向いていないと、そもそもホバリングしない

[tmori]

ヨー角：90度の場合

現象：QGCから離陸指示を出すと、プロペラは回転するが、浮上しない。そして、どっかへ飛んでいってしまう。

ログ：

log_90deg.zip

ヨー角：180度の場合

現象：QGCから離陸指示も出していないのに、シミュレーション開始と同時に、浮上し始める。。

ログ：

log_180deg.zip

rotor, thrustのログを確認しましたが、浮上指示は出していないっぽい。

ヨー角：270度の場合

現象：離陸指示を出すとQGCが「離陸することができません。機体位置が不明です。」とエラーメッセージを出す。
さらに、PX4からは以下の警告が出力される。

WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: vertical velocity unstable
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias

ログ：

log_270deg.zip

[kenjihiranabe]

やったじゃん！これが原因だね。 ここまでわかれば後は行けるでしょう。

[tmori]

ヨー角：180度の場合の原因は、これでした。

#89

暫定修正して、ホバリングすることを確認済みです。

ヨー角：90の場合も、この修正を適用することで、ホバリングすることを確認。

ヨー角：270の場合も、この修正を適用することで、ホバリングすることを確認。

[kenjihiranabe]

教訓：今回なぜ単体テストをすり抜けたか。
全方向に body を回してテストいるが、最初のベクトルが、v = {1, 0, 0 } だった。
この y=z=0 ベクトルはいくらどの方向に回しても、このバグはキャンセルされてでない。

   z_e =   (- s_theta)                                 * x
           + (s_psi * c_theta)                         * y
           + (c_psi * c_theta)                         * z;

なので。テストを追加しました。