Diff: Autopilot.hpp

Revision:

0:e9cdfc6579a7

Child:

1:73704460a8b4

diff -r 000000000000 -r e9cdfc6579a7 Autopilot.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Autopilot.hpp	Fri Nov 12 06:50:45 2021 +0000
@@ -0,0 +1,52 @@
+#ifndef __AUTOPILOT_HPP__
+#define __AUTOPILOT_HPP__
+
+#include "mbed.h"
+#include "Vector3.hpp"
+#include <vector>
+#include <cmath>
+
+#define pi 3.141592f;
+
+class Autopilot
+{
+private:
+    //観測値
+    float roll;
+    float pitch;
+    float yaw;
+    float alt;
+    Vector3 pos_ned;
+    
+    //目標値
+    float roll_obj;
+    float pitch_obj;
+    float yaw_obj;
+    float alt_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 deg2rad(float deg);   //degからradに変換
+        
+public:
+    Autopilot();
+    
+    void update_val(Vector3 rpy, float altitude, Vector3 pos);   //姿勢角等の更新
+    
+    void level();    //水平飛行
+    void guide();    //定点誘導
+    void turn();     //定点旋回(時計回り)
+    void keep_alt();    //高度維持
+    
+    void set_dest(Vector3 dest);    //誘導地点の設定
+    void set_turn(Vector3 center, float r);  //定点旋回の設定
+    
+    std::vector<float> return_val();     //目標値を返す
+}
+
+#endif
\ No newline at end of file