Diff: Autopilot.hpp

Revision:

14:7b6c3f8b85fb

Parent:

12:59e547742cd8

Child:

15:28f480b41553

--- 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);     //目標値を代入
 };