Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.hpp@15:28f480b41553, 2021-12-06 (annotated)
- Committer:
- cocorlow
- Date:
- Mon Dec 06 08:23:54 2021 +0000
- Revision:
- 15:28f480b41553
- Parent:
- 14:7b6c3f8b85fb
- Child:
- 17:598522e1bf4f
Autopilot solaESKF
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" |
cocorlow | 15:28f480b41553 | 6 | #include <Eigen/Core.h> |
osaka | 0:e9cdfc6579a7 | 7 | #include <cmath> |
osaka | 0:e9cdfc6579a7 | 8 | |
cocorlow | 15:28f480b41553 | 9 | using namespace Eigen; |
cocorlow | 15:28f480b41553 | 10 | |
cocorlow | 15:28f480b41553 | 11 | #ifndef M_PI_F |
cocorlow | 15:28f480b41553 | 12 | #define M_PI_F 3.141592f |
cocorlow | 15:28f480b41553 | 13 | #endif |
cocorlow | 15:28f480b41553 | 14 | |
cocorlow | 15:28f480b41553 | 15 | #ifndef G |
osaka | 6:1f5e6efff5b4 | 16 | #define G 9.8f |
cocorlow | 15:28f480b41553 | 17 | #endif |
osaka | 0:e9cdfc6579a7 | 18 | |
osaka | 0:e9cdfc6579a7 | 19 | class Autopilot |
osaka | 0:e9cdfc6579a7 | 20 | { |
osaka | 0:e9cdfc6579a7 | 21 | private: |
osaka | 0:e9cdfc6579a7 | 22 | //観測値 |
osaka | 0:e9cdfc6579a7 | 23 | float roll; |
osaka | 0:e9cdfc6579a7 | 24 | float pitch; |
osaka | 0:e9cdfc6579a7 | 25 | float yaw; |
osaka | 0:e9cdfc6579a7 | 26 | float alt; |
osaka | 14:7b6c3f8b85fb | 27 | float alt_before; |
cocorlow | 15:28f480b41553 | 28 | Vector3f pos_ned; |
cocorlow | 15:28f480b41553 | 29 | Vector3f pos_before; |
cocorlow | 15:28f480b41553 | 30 | Vector3f vel_ned; //対地速度 |
cocorlow | 15:28f480b41553 | 31 | Vector3f vel_before; |
osaka | 14:7b6c3f8b85fb | 32 | |
osaka | 14:7b6c3f8b85fb | 33 | //算出値 |
osaka | 14:7b6c3f8b85fb | 34 | float path_ang; //経路角 |
cocorlow | 15:28f480b41553 | 35 | Vector3f vdot; |
osaka | 0:e9cdfc6579a7 | 36 | |
osaka | 0:e9cdfc6579a7 | 37 | //目標値 |
osaka | 0:e9cdfc6579a7 | 38 | float roll_obj; |
osaka | 0:e9cdfc6579a7 | 39 | float pitch_obj; |
osaka | 0:e9cdfc6579a7 | 40 | float yaw_obj; |
osaka | 0:e9cdfc6579a7 | 41 | float alt_obj; |
osaka | 6:1f5e6efff5b4 | 42 | float vel_obj; //対地速度 |
osaka | 6:1f5e6efff5b4 | 43 | float dT_obj; |
osaka | 14:7b6c3f8b85fb | 44 | float path_obj; |
osaka | 14:7b6c3f8b85fb | 45 | float vdot_obj; |
osaka | 0:e9cdfc6579a7 | 46 | |
cocorlow | 15:28f480b41553 | 47 | Vector3f destination; //誘導地点NED座標 |
cocorlow | 15:28f480b41553 | 48 | Vector3f turn_center; //旋回中心NED座標 |
osaka | 0:e9cdfc6579a7 | 49 | float turn_r; //旋回半径 |
osaka | 0:e9cdfc6579a7 | 50 | |
osaka | 0:e9cdfc6579a7 | 51 | void limit_obj(); |
osaka | 6:1f5e6efff5b4 | 52 | float p_control(float diff, float kp); //比例制御(diff:目標値-現在値, kp:比例ゲイン) |
osaka | 6:1f5e6efff5b4 | 53 | float angdiff_pi(float rad); //角度を[-pi, pi]の範囲で出力 |
NaotoMorita | 11:51a0fedd7745 | 54 | float atan_angdiff(float a1,float a2); //atan2を用いた角度差計算 |
osaka | 0:e9cdfc6579a7 | 55 | float deg2rad(float deg); //degからradに変換 |
osaka | 14:7b6c3f8b85fb | 56 | float calc_path(); //経路角算出 |
osaka | 0:e9cdfc6579a7 | 57 | |
osaka | 0:e9cdfc6579a7 | 58 | public: |
osaka | 0:e9cdfc6579a7 | 59 | Autopilot(); |
osaka | 6:1f5e6efff5b4 | 60 | |
osaka | 6:1f5e6efff5b4 | 61 | void set_dest(float x, float y); //誘導地点の設定 |
osaka | 6:1f5e6efff5b4 | 62 | void set_turn(float x, float y, float r); //定点旋回の設定 |
osaka | 6:1f5e6efff5b4 | 63 | void set_alt(float alt, float vel); //高度,速度の設定 |
osaka | 14:7b6c3f8b85fb | 64 | void set_climb(float path_angle, float vdot); //経路角,加速度の設定 |
osaka | 0:e9cdfc6579a7 | 65 | |
cocorlow | 15:28f480b41553 | 66 | void update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel, const Vector3f v_dot); //姿勢角等の更新 |
osaka | 0:e9cdfc6579a7 | 67 | |
osaka | 0:e9cdfc6579a7 | 68 | void level(); //水平飛行 |
osaka | 0:e9cdfc6579a7 | 69 | void guide(); //定点誘導 |
osaka | 0:e9cdfc6579a7 | 70 | void turn(); //定点旋回(時計回り) |
osaka | 6:1f5e6efff5b4 | 71 | void keep_alt(); //高度,速度維持 |
osaka | 14:7b6c3f8b85fb | 72 | void climb(); //経路角一定 |
osaka | 0:e9cdfc6579a7 | 73 | |
osaka | 6:1f5e6efff5b4 | 74 | void return_val(float &r_obj, float &p_obj, float &t_obj); //目標値を代入 |
osaka | 1:73704460a8b4 | 75 | }; |
osaka | 0:e9cdfc6579a7 | 76 | |
osaka | 0:e9cdfc6579a7 | 77 | #endif |