完了条件：

• main関数を追加する
• #46 の対応を行う
• #47 の対応を行う
• #48 の対応を行う
• MAVLINKの単位を揃える
• PX4と結合し、takeoffコマンドでドローンが反応すること（少なくともSIGSEGV等で落ちないこと）

以下のメッセージの設定値を、AirSimのパケットデータをベースに見直しする。
見直しの結果、修正不要な場合もある。

• HIL_SENSOR
• HIL_GPS

箱庭のPX4コマンド(px4_hakoniwa start)で浮上させても、Landing detected判定されて、落下してしまう。

PX4のLanding detectedの判定条件が不明なため、調査ペンディングした。

回避方法としては、COM_DISARM_LANDを-1にして回避できる。

param set COM_DISARM_LAND -1

https://docs.px4.io/v1.11/en/advanced_config/parameter_reference.html

ROSの座標系から航空座標系への変換式：

現時点(2023/11/24)で、PX4と箱庭の物理モデル（DronePhysics）との通信齟齬があり、PX4からの指示で箱庭上のドローンの姿勢制御が安定しないという問題がある。

箱庭の物理モデルは以下の通り。

安定しない原因は、以下のいずれかが考えられる。

• HIL_SENSORの設定値に誤りがある
• HIL_GPSの設定値に誤りがある
• HIL_ACTUATOR_CONTROLSからモーター駆動する物理処理部に誤りがある
• ドローンの物理式がPX4の想定と異なる

(※) AirSimとPX4との間の通信パケット調査した結果、上記３メッセージのみが制御に関係していることを確認した。

そこで、問題の所在を明らかにするために、下図の通り、箱庭上の物理処理部分をコンポーネント分割し、各コンポーネントの入出力仕様を明らかにする。まずは、こちらの想定で仕様を仮定義し、第三者がチェックしやすいドキュメントを整備する。

現象として、クォータニオンからオイラー角に変換すると、Y軸とZ軸の角度の符号が逆転していることを確認した。

該当ファイルは以下の通り。以下のずれかがずれているものと思われる。

• euler2Quaternion
• quaternion2Euler

• HIL_SENSOR
• HIL_GPS
• HIL_ACTUATOR_CONTROLS

• 送信データのSystemIDとComponentIDの見直し
• 各パケットの送信周期の見直し

センサのデータ値は、座標系を知らないと適切な値を設定できないものがある。

以下のデータを現時点では送信しているが、各メンバをチェックし、仮説としての仕様を定義する。
問題があれば、この仕様とコードを一致させたい。

• HIL_GPS
• HIL_SENSORS
• HIL_STATE_QUATERNION

• 各種センサに必要なノイズの選定と設定を行う。
• 送信スレッド側でノイズを入れているが、削除する。

原因：

• Mavlinkヘッダ部分の警告
• Pythonヘッダ部分の警告

以下の情報をサマリデータとしてまずは解析する。

• 流れているパケットの種類
• 流れているパケットの送信周期
• 流れているパケットのシーケンス
• 流れているパケットのデータ（代表値で良い）

• HIL_STATE_QUATERNIONの送信処理削除
• HEARTBEAT送信の送信処理削除

大枠の設計概要図：

作成するもの：

• bypass 引数指定すると、上図の通りパケットキャプチャしてくれる機能
• キャプチャしたデータ（バイナリ）をテキスト変換してくれる機能（リアルタイムでなくて良い）

• COMMAND_LONGの設定値応答追加

作業タスク一覧

• 箱庭のdocker ファイルを作成する
• Windows版Unity環境を作成する
• PX4のdockerコンテナをWindows向けにカスタマイズする
• 動作チェックする

px4_hakoniwaは、現状、vehicle_global_position_groundtruth を参照してZ軸の位置のPID制御をしている。

しかし、PX4標準のモジュール群は、カルマンフィルタ (ekfs) が生成する vehicle_local_position を参照して、制御を行っている。

そのため、 px4_hakoniwa でも、同様に vehicle_local_position で位置制御できるようにしたい。

以下の対応を行う予定

• MATLAB/Simulink用の箱庭APIモデル作成
• プラント側と制御側、それぞれのモデルで箱庭APIモデルと繋げてコード生成できるようにする
• 生成されたコードを箱庭上で動作できるようにする

前提：

• 箱庭のシミュレータのドローン機体
• PX4標準の機体とは異なる（SYS_AUTOSTART: 10016）

状況：

箱庭のシミュレータのドローン機体の入力と変換部分を、10016 向けに対応しているが、うまくいかない。。

アクチュエータ側のチェック

与えられた入力（PWMデューティ比）に対して、機体が適切な方向に移動しているかどうを総合的にチェックする。

単体テストでは、限られた入力でのチェックしかしていないので、シミュレーションデータでの妥当性をチェック。

機体の位置

作戦：

• ローター回転数→推力→高さのプロットをして関係性をチェック

機体の姿勢

作戦：

• ローター回転数→ロール側のトルク→姿勢角・φ
• ローター回転数→ピッチ側のトルク→姿勢角・θ
• ローター回転数→ヨー側のトルク→姿勢角・ψ

センサー側のチェック

磁気センサ（チェック方法検討中）

気圧センサ（チェック方法検討中）

ジャイロセンサ（チェック方法検討中）

加速度センサ（チェック方法検討中）

GPSセンサ（チェック方法検討中）

三角関数の呼び出しはコスト高いので，同じ x について cos(x) などが複数回（特にループで）呼ばれるなら，cx= cos(x) などと一回メモリに載せておくこと。

シミュレーション全体

• シミュレーション時間ステップ
• ログデータ出力ディレクトリパス
• ログ出力をマスクするコンポーネント
• 各種MAVLINKデータの送信周期

ドローンダイナミクス

• 地上座標系の物理式/機体座標系の物理式
• 空気摩擦係数
• トルク係数

ローター

• 時定数τ
• k
• rpm_max

スラスター

• ローターの位置と回転方向
• パラメータA
• パラメータB
• パラメータJr

ACC

• サンプル数
• ノイズ

Gyro

• サンプル数
• ノイズ

Mag

• サンプル数
• ノイズ
• 磁力量、偏角、俯角

BARO

• サンプル数
• ノイズ
• 緯度、経度、高度

GPS

• サンプル数
• ノイズ
• 緯度、経度、高度

以下を作成する。

• ディレクトリ階層
• トップのREADME
• アーキテクチャを載せる
• アーキテクチャの構成要素一覧と概要を列挙する

こちら：

https://github.com/toppers/hakoniwa-px4sim/blob/main/hakoniwa/src/assets/drone/physics/body_frame/drone_dynamics_body_frame.hpp

https://github.com/toppers/hakoniwa-px4sim/blob/c1d1faf51ad90d2d1f7a9ddff0fce92759e47217/hakoniwa/src/assets/drone/physics/ground_frame/drone_dynamics_ground_frame.cpp#L30C149-L30C150

s_psy は c_psi の間違いっぽい。。。確認ください。

以下の不具合を対応：

• HIL_STATE_QUATERNION
  • zacc には、重力加速度は不要
  • zacc は、航空座標系のため、浮上する場合はマイナス値になるので符号を逆転する必要がある
• HIL_SENSORS
  • zacc には、重力加速度が必要
  • zaccは、ROS座標系のため、浮上する場合はプラス値になる（変更不要だが、HIL_SATETE_QUATERNIONの修正影響を受けるのでその対応を実施）

以下の機能改善を対応：

• 姿勢データを平均値としてセンシングできるようにする