HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.hpp@11:51a0fedd7745, 2021-11-29 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 29 09:45:32 2021 +0000
Revision:
11:51a0fedd7745
Parent:
6:1f5e6efff5b4
Child:
12:59e547742cd8
yaw obj mod

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