Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Diff: Autopilot.hpp
- Revision:
- 14:7b6c3f8b85fb
- Parent:
- 12:59e547742cd8
- Child:
- 15:28f480b41553
diff -r 75636841e43b -r 7b6c3f8b85fb Autopilot.hpp --- a/Autopilot.hpp Wed Dec 01 09:23:26 2021 +0000 +++ b/Autopilot.hpp Wed Dec 01 11:51:21 2021 +0000 @@ -17,8 +17,15 @@ float pitch; float yaw; float alt; + float alt_before; Vector3 pos_ned; + Vector3 pos_before; Vector3 vel_ned; //対地速度 + Vector3 vel_before; + + //算出値 + float path_ang; //経路角 + Vector3 vdot; //目標値 float roll_obj; @@ -27,6 +34,8 @@ float alt_obj; float vel_obj; //対地速度 float dT_obj; + float path_obj; + float vdot_obj; Vector3 destination; //誘導地点NED座標 Vector3 turn_center; //旋回中心NED座標 @@ -37,6 +46,7 @@ float angdiff_pi(float rad); //角度を[-pi, pi]の範囲で出力 float atan_angdiff(float a1,float a2); //atan2を用いた角度差計算 float deg2rad(float deg); //degからradに変換 + float calc_path(); //経路角算出 public: Autopilot(); @@ -44,13 +54,15 @@ void set_dest(float x, float y); //誘導地点の設定 void set_turn(float x, float y, float r); //定点旋回の設定 void set_alt(float alt, float vel); //高度,速度の設定 + void set_climb(float path_angle, float vdot); //経路角,加速度の設定 - void update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel); //姿勢角等の更新 + void update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot); //姿勢角等の更新 void level(); //水平飛行 void guide(); //定点誘導 void turn(); //定点旋回(時計回り) void keep_alt(); //高度,速度維持 + void climb(); //経路角一定 void return_val(float &r_obj, float &p_obj, float &t_obj); //目標値を代入 };