HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.hpp@2:049e057f4625, 2021-11-12 (annotated)

Committer:
osaka
Date:
Fri Nov 12 12:16:38 2021 +0000
Revision:
2:049e057f4625
Parent:
1:73704460a8b4
Child:
3:46d4a32011fc
debugged

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 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 0:e9cdfc6579a7 20
osaka 0:e9cdfc6579a7 21 //目標値
osaka 0:e9cdfc6579a7 22 float roll_obj;
osaka 0:e9cdfc6579a7 23 float pitch_obj;
osaka 0:e9cdfc6579a7 24 float yaw_obj;
osaka 0:e9cdfc6579a7 25 float alt_obj;
osaka 0:e9cdfc6579a7 26
osaka 0:e9cdfc6579a7 27 Vector3 destination; //誘導地点NED座標
osaka 0:e9cdfc6579a7 28 Vector3 turn_center; //旋回中心NED座標
osaka 0:e9cdfc6579a7 29 float turn_r; //旋回半径
osaka 0:e9cdfc6579a7 30
osaka 0:e9cdfc6579a7 31 void limit_obj();
osaka 0:e9cdfc6579a7 32 float p_control(float dif, float kp); //比例制御(dif:目標値-現在値, kp:比例ゲイン)
osaka 0:e9cdfc6579a7 33 float angdif_pi(float rad); //角度を[-pi, pi]の範囲で出力
osaka 0:e9cdfc6579a7 34 float deg2rad(float deg); //degからradに変換
osaka 0:e9cdfc6579a7 35
osaka 0:e9cdfc6579a7 36 public:
osaka 0:e9cdfc6579a7 37 Autopilot();
osaka 0:e9cdfc6579a7 38
osaka 0:e9cdfc6579a7 39 void update_val(Vector3 rpy, float altitude, Vector3 pos); //姿勢角等の更新
osaka 0:e9cdfc6579a7 40
osaka 0:e9cdfc6579a7 41 void level(); //水平飛行
osaka 0:e9cdfc6579a7 42 void guide(); //定点誘導
osaka 0:e9cdfc6579a7 43 void turn(); //定点旋回(時計回り)
osaka 0:e9cdfc6579a7 44 void keep_alt(); //高度維持
osaka 0:e9cdfc6579a7 45
osaka 1:73704460a8b4 46 void set_dest(float x, float y); //誘導地点の設定
osaka 1:73704460a8b4 47 void set_turn(float x, float y, float r); //定点旋回の設定
osaka 0:e9cdfc6579a7 48
osaka 2:049e057f4625 49 void return_val(float &r_obj, float &p_obj, float &a_obj); //目標値を代入
osaka 1:73704460a8b4 50 };
osaka 0:e9cdfc6579a7 51
osaka 0:e9cdfc6579a7 52 #endif