HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.hpp@14:7b6c3f8b85fb, 2021-12-01 (annotated)

Committer:
osaka
Date:
Wed Dec 01 11:51:21 2021 +0000
Revision:
14:7b6c3f8b85fb
Parent:
12:59e547742cd8
Child:
15:28f480b41553
with path angle control

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 12:59e547742cd8 6 #include "Matrix.h"
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 14:7b6c3f8b85fb 20 float alt_before;
osaka 0:e9cdfc6579a7 21 Vector3 pos_ned;
osaka 14:7b6c3f8b85fb 22 Vector3 pos_before;
osaka 6:1f5e6efff5b4 23 Vector3 vel_ned; //対地速度
osaka 14:7b6c3f8b85fb 24 Vector3 vel_before;
osaka 14:7b6c3f8b85fb 25
osaka 14:7b6c3f8b85fb 26 //算出値
osaka 14:7b6c3f8b85fb 27 float path_ang; //経路角
osaka 14:7b6c3f8b85fb 28 Vector3 vdot;
osaka 0:e9cdfc6579a7 29
osaka 0:e9cdfc6579a7 30 //目標値
osaka 0:e9cdfc6579a7 31 float roll_obj;
osaka 0:e9cdfc6579a7 32 float pitch_obj;
osaka 0:e9cdfc6579a7 33 float yaw_obj;
osaka 0:e9cdfc6579a7 34 float alt_obj;
osaka 6:1f5e6efff5b4 35 float vel_obj; //対地速度
osaka 6:1f5e6efff5b4 36 float dT_obj;
osaka 14:7b6c3f8b85fb 37 float path_obj;
osaka 14:7b6c3f8b85fb 38 float vdot_obj;
osaka 0:e9cdfc6579a7 39
osaka 0:e9cdfc6579a7 40 Vector3 destination; //誘導地点NED座標
osaka 0:e9cdfc6579a7 41 Vector3 turn_center; //旋回中心NED座標
osaka 0:e9cdfc6579a7 42 float turn_r; //旋回半径
osaka 0:e9cdfc6579a7 43
osaka 0:e9cdfc6579a7 44 void limit_obj();
osaka 6:1f5e6efff5b4 45 float p_control(float diff, float kp); //比例制御(diff:目標値-現在値, kp:比例ゲイン)
osaka 6:1f5e6efff5b4 46 float angdiff_pi(float rad); //角度を[-pi, pi]の範囲で出力
NaotoMorita 11:51a0fedd7745 47 float atan_angdiff(float a1,float a2); //atan2を用いた角度差計算
osaka 0:e9cdfc6579a7 48 float deg2rad(float deg); //degからradに変換
osaka 14:7b6c3f8b85fb 49 float calc_path(); //経路角算出
osaka 0:e9cdfc6579a7 50
osaka 0:e9cdfc6579a7 51 public:
osaka 0:e9cdfc6579a7 52 Autopilot();
osaka 6:1f5e6efff5b4 53
osaka 6:1f5e6efff5b4 54 void set_dest(float x, float y); //誘導地点の設定
osaka 6:1f5e6efff5b4 55 void set_turn(float x, float y, float r); //定点旋回の設定
osaka 6:1f5e6efff5b4 56 void set_alt(float alt, float vel); //高度，速度の設定
osaka 14:7b6c3f8b85fb 57 void set_climb(float path_angle, float vdot); //経路角,加速度の設定
osaka 0:e9cdfc6579a7 58
osaka 14:7b6c3f8b85fb 59 void update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot); //姿勢角等の更新
osaka 0:e9cdfc6579a7 60
osaka 0:e9cdfc6579a7 61 void level(); //水平飛行
osaka 0:e9cdfc6579a7 62 void guide(); //定点誘導
osaka 0:e9cdfc6579a7 63 void turn(); //定点旋回(時計回り)
osaka 6:1f5e6efff5b4 64 void keep_alt(); //高度，速度維持
osaka 14:7b6c3f8b85fb 65 void climb(); //経路角一定
osaka 0:e9cdfc6579a7 66
osaka 6:1f5e6efff5b4 67 void return_val(float &r_obj, float &p_obj, float &t_obj); //目標値を代入
osaka 1:73704460a8b4 68 };
osaka 0:e9cdfc6579a7 69
osaka 0:e9cdfc6579a7 70 #endif