Diff: Autopilot.hpp

Revision:

6:1f5e6efff5b4

Parent:

3:46d4a32011fc

Child:

8:7f3ff375fd30

Child:

11:51a0fedd7745

--- a/Autopilot.hpp	Mon Nov 15 13:41:51 2021 +0000
+++ b/Autopilot.hpp	Fri Nov 19 05:18:37 2021 +0000
@@ -7,6 +7,7 @@
 #include <cmath>
 
 #define M_PI 3.141592f
+#define G 9.8f
 
 class Autopilot
 {
@@ -17,36 +18,40 @@
     float yaw;
     float alt;
     Vector3 pos_ned;
+    Vector3 vel_ned;    //対地速度
     
     //目標値
     float roll_obj;
     float pitch_obj;
     float yaw_obj;
     float alt_obj;
+    float vel_obj;  //対地速度
+    float dT_obj;
     
     Vector3 destination;    //誘導地点NED座標
     Vector3 turn_center;    //旋回中心NED座標
     float turn_r;   //旋回半径
     
     void limit_obj();
-    float p_control(float dif, float kp);    //比例制御(dif:目標値-現在値, kp:比例ゲイン)
-    float angdif_pi(float rad);    //角度を[-pi, pi]の範囲で出力
+    float p_control(float diff, float kp);    //比例制御(diff:目標値-現在値, kp:比例ゲイン)
+    float angdiff_pi(float rad);    //角度を[-pi, pi]の範囲で出力
     float deg2rad(float deg);   //degからradに変換
         
 public:
     Autopilot();
+        
+    void set_dest(float x, float y);    //誘導地点の設定
+    void set_turn(float x, float y, float r);  //定点旋回の設定
+    void set_alt(float alt, float vel);     //高度，速度の設定
     
-    void update_val(const Vector3 rpy, const float altitude, const Vector3 pos);   //姿勢角等の更新
+    void update_val(const Vector3 rpy, const float altitude, const Vector3 pos, const Vector3 vel);   //姿勢角等の更新
     
     void level();    //水平飛行
     void guide();    //定点誘導
     void turn();     //定点旋回(時計回り)
-    void keep_alt();    //高度維持
+    void keep_alt();    //高度，速度維持
     
-    void set_dest(float x, float y);    //誘導地点の設定
-    void set_turn(float x, float y, float r);  //定点旋回の設定
-    
-    void return_val(float &r_obj, float &p_obj, float &a_obj);     //目標値を代入
+    void return_val(float &r_obj, float &p_obj, float &t_obj);     //目標値を代入
 };
 
 #endif
\ No newline at end of file