HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.hpp@19:86ed9a546684, 23 months ago (annotated)

Committer:
NaotoMorita
Date:
Thu Jul 07 13:56:52 2022 +0000
Revision:
19:86ed9a546684
Parent:
18:5e2e03a30e1c 
new hil

Who changed what in which revision?

UserRevisionLine numberNew 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"
cocorlow 17:598522e1bf4f 5 #include <Eigen/Dense.h>
cocorlow 17:598522e1bf4f 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;
NaotoMorita 18:5e2e03a30e1c 44 //float path_obj;
NaotoMorita 18:5e2e03a30e1c 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に変換
NaotoMorita 18:5e2e03a30e1c 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); //高度，速度の設定
NaotoMorita 18:5e2e03a30e1c 64 //void set_climb(float path_angle, float vdot); //経路角,加速度の設定
osaka 0:e9cdfc6579a7 65
NaotoMorita 18:5e2e03a30e1c 66 void update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel); //姿勢角等の更新
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(); //高度，速度維持
NaotoMorita 18:5e2e03a30e1c 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