Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.hpp
- Committer:
- osaka
- Date:
- 2021-11-12
- Revision:
- 2:049e057f4625
- Parent:
- 1:73704460a8b4
- Child:
- 3:46d4a32011fc
File content as of revision 2:049e057f4625:
#ifndef __AUTOPILOT_HPP__ #define __AUTOPILOT_HPP__ #include "mbed.h" #include "Vector3.hpp" #include <vector> #include <cmath> #define M_PI 3.141592f class Autopilot { private: //観測値 float roll; float pitch; float yaw; float alt; Vector3 pos_ned; //目標値 float roll_obj; float pitch_obj; float yaw_obj; float alt_obj; Vector3 destination; //誘導地点NED座標 Vector3 turn_center; //旋回中心NED座標 float turn_r; //旋回半径 void limit_obj(); float p_control(float dif, float kp); //比例制御(dif:目標値-現在値, kp:比例ゲイン) float angdif_pi(float rad); //角度を[-pi, pi]の範囲で出力 float deg2rad(float deg); //degからradに変換 public: Autopilot(); void update_val(Vector3 rpy, float altitude, Vector3 pos); //姿勢角等の更新 void level(); //水平飛行 void guide(); //定点誘導 void turn(); //定点旋回(時計回り) void keep_alt(); //高度維持 void set_dest(float x, float y); //誘導地点の設定 void set_turn(float x, float y, float r); //定点旋回の設定 void return_val(float &r_obj, float &p_obj, float &a_obj); //目標値を代入 }; #endif