HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.hpp@8:7f3ff375fd30, 2021-11-19 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 19 08:22:01 2021 +0000
Revision:
8:7f3ff375fd30
Parent:
6:1f5e6efff5b4 
heading fuse

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"
osaka 0:e9cdfc6579a7 5 #include "Vector3.hpp"
osaka 0:e9cdfc6579a7 6 #include <cmath>
osaka 0:e9cdfc6579a7 7
osaka 1:73704460a8b4 8 #define M_PI 3.141592f
osaka 6:1f5e6efff5b4 9 #define G 9.8f
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 6:1f5e6efff5b4 20 Vector3 vel_ned; //対地速度
osaka 0:e9cdfc6579a7 21
osaka 0:e9cdfc6579a7 22 //目標値
osaka 0:e9cdfc6579a7 23 float roll_obj;
osaka 0:e9cdfc6579a7 24 float pitch_obj;
osaka 0:e9cdfc6579a7 25 float yaw_obj;
osaka 0:e9cdfc6579a7 26 float alt_obj;
osaka 6:1f5e6efff5b4 27 float vel_obj; //対地速度
osaka 6:1f5e6efff5b4 28 float dT_obj;
osaka 0:e9cdfc6579a7 29
osaka 0:e9cdfc6579a7 30 Vector3 destination; //誘導地点NED座標
osaka 0:e9cdfc6579a7 31 Vector3 turn_center; //旋回中心NED座標
osaka 0:e9cdfc6579a7 32 float turn_r; //旋回半径
osaka 0:e9cdfc6579a7 33
osaka 0:e9cdfc6579a7 34 void limit_obj();
osaka 6:1f5e6efff5b4 35 float p_control(float diff, float kp); //比例制御(diff:目標値-現在値, kp:比例ゲイン)
osaka 6:1f5e6efff5b4 36 float angdiff_pi(float rad); //角度を[-pi, pi]の範囲で出力
osaka 0:e9cdfc6579a7 37 float deg2rad(float deg); //degからradに変換
osaka 0:e9cdfc6579a7 38
osaka 0:e9cdfc6579a7 39 public:
osaka 0:e9cdfc6579a7 40 Autopilot();
osaka 6:1f5e6efff5b4 41
osaka 6:1f5e6efff5b4 42 void set_dest(float x, float y); //誘導地点の設定
osaka 6:1f5e6efff5b4 43 void set_turn(float x, float y, float r); //定点旋回の設定
osaka 6:1f5e6efff5b4 44 void set_alt(float alt, float vel); //高度，速度の設定
osaka 0:e9cdfc6579a7 45
osaka 6:1f5e6efff5b4 46 void update_val(const Vector3 rpy, const float altitude, const Vector3 pos, const Vector3 vel); //姿勢角等の更新
osaka 0:e9cdfc6579a7 47
osaka 0:e9cdfc6579a7 48 void level(); //水平飛行
osaka 0:e9cdfc6579a7 49 void guide(); //定点誘導
osaka 0:e9cdfc6579a7 50 void turn(); //定点旋回(時計回り)
osaka 6:1f5e6efff5b4 51 void keep_alt(); //高度，速度維持
osaka 0:e9cdfc6579a7 52
osaka 6:1f5e6efff5b4 53 void return_val(float &r_obj, float &p_obj, float &t_obj); //目標値を代入
osaka 1:73704460a8b4 54 };
osaka 0:e9cdfc6579a7 55
osaka 0:e9cdfc6579a7 56 #endif