Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.hpp@2:049e057f4625, 2021-11-12 (annotated)
- Committer:
- osaka
- Date:
- Fri Nov 12 12:16:38 2021 +0000
- Revision:
- 2:049e057f4625
- Parent:
- 1:73704460a8b4
- Child:
- 3:46d4a32011fc
debugged
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
osaka | 0:e9cdfc6579a7 | 1 | #ifndef __AUTOPILOT_HPP__ |
osaka | 0:e9cdfc6579a7 | 2 | #define __AUTOPILOT_HPP__ |
osaka | 0:e9cdfc6579a7 | 3 | |
osaka | 0:e9cdfc6579a7 | 4 | #include "mbed.h" |
osaka | 0:e9cdfc6579a7 | 5 | #include "Vector3.hpp" |
osaka | 0:e9cdfc6579a7 | 6 | #include <vector> |
osaka | 0:e9cdfc6579a7 | 7 | #include <cmath> |
osaka | 0:e9cdfc6579a7 | 8 | |
osaka | 1:73704460a8b4 | 9 | #define M_PI 3.141592f |
osaka | 0:e9cdfc6579a7 | 10 | |
osaka | 0:e9cdfc6579a7 | 11 | class Autopilot |
osaka | 0:e9cdfc6579a7 | 12 | { |
osaka | 0:e9cdfc6579a7 | 13 | private: |
osaka | 0:e9cdfc6579a7 | 14 | //観測値 |
osaka | 0:e9cdfc6579a7 | 15 | float roll; |
osaka | 0:e9cdfc6579a7 | 16 | float pitch; |
osaka | 0:e9cdfc6579a7 | 17 | float yaw; |
osaka | 0:e9cdfc6579a7 | 18 | float alt; |
osaka | 0:e9cdfc6579a7 | 19 | Vector3 pos_ned; |
osaka | 0:e9cdfc6579a7 | 20 | |
osaka | 0:e9cdfc6579a7 | 21 | //目標値 |
osaka | 0:e9cdfc6579a7 | 22 | float roll_obj; |
osaka | 0:e9cdfc6579a7 | 23 | float pitch_obj; |
osaka | 0:e9cdfc6579a7 | 24 | float yaw_obj; |
osaka | 0:e9cdfc6579a7 | 25 | float alt_obj; |
osaka | 0:e9cdfc6579a7 | 26 | |
osaka | 0:e9cdfc6579a7 | 27 | Vector3 destination; //誘導地点NED座標 |
osaka | 0:e9cdfc6579a7 | 28 | Vector3 turn_center; //旋回中心NED座標 |
osaka | 0:e9cdfc6579a7 | 29 | float turn_r; //旋回半径 |
osaka | 0:e9cdfc6579a7 | 30 | |
osaka | 0:e9cdfc6579a7 | 31 | void limit_obj(); |
osaka | 0:e9cdfc6579a7 | 32 | float p_control(float dif, float kp); //比例制御(dif:目標値-現在値, kp:比例ゲイン) |
osaka | 0:e9cdfc6579a7 | 33 | float angdif_pi(float rad); //角度を[-pi, pi]の範囲で出力 |
osaka | 0:e9cdfc6579a7 | 34 | float deg2rad(float deg); //degからradに変換 |
osaka | 0:e9cdfc6579a7 | 35 | |
osaka | 0:e9cdfc6579a7 | 36 | public: |
osaka | 0:e9cdfc6579a7 | 37 | Autopilot(); |
osaka | 0:e9cdfc6579a7 | 38 | |
osaka | 0:e9cdfc6579a7 | 39 | void update_val(Vector3 rpy, float altitude, Vector3 pos); //姿勢角等の更新 |
osaka | 0:e9cdfc6579a7 | 40 | |
osaka | 0:e9cdfc6579a7 | 41 | void level(); //水平飛行 |
osaka | 0:e9cdfc6579a7 | 42 | void guide(); //定点誘導 |
osaka | 0:e9cdfc6579a7 | 43 | void turn(); //定点旋回(時計回り) |
osaka | 0:e9cdfc6579a7 | 44 | void keep_alt(); //高度維持 |
osaka | 0:e9cdfc6579a7 | 45 | |
osaka | 1:73704460a8b4 | 46 | void set_dest(float x, float y); //誘導地点の設定 |
osaka | 1:73704460a8b4 | 47 | void set_turn(float x, float y, float r); //定点旋回の設定 |
osaka | 0:e9cdfc6579a7 | 48 | |
osaka | 2:049e057f4625 | 49 | void return_val(float &r_obj, float &p_obj, float &a_obj); //目標値を代入 |
osaka | 1:73704460a8b4 | 50 | }; |
osaka | 0:e9cdfc6579a7 | 51 | |
osaka | 0:e9cdfc6579a7 | 52 | #endif |