Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.hpp
- Committer:
- cocorlow
- Date:
- 2021-12-10
- Revision:
- 17:598522e1bf4f
- Parent:
- 15:28f480b41553
- Child:
- 18:5e2e03a30e1c
File content as of revision 17:598522e1bf4f:
#ifndef __AUTOPILOT_HPP__ #define __AUTOPILOT_HPP__ #include "mbed.h" #include "Vector3.hpp" #include <Eigen/Dense.h> //#include <Eigen/Core.h> #include <cmath> using namespace Eigen; #ifndef M_PI_F #define M_PI_F 3.141592f #endif #ifndef G #define G 9.8f #endif class Autopilot { private: //観測値 float roll; float pitch; float yaw; float alt; float alt_before; Vector3f pos_ned; Vector3f pos_before; Vector3f vel_ned; //対地速度 Vector3f vel_before; //算出値 float path_ang; //経路角 Vector3f vdot; //目標値 float roll_obj; float pitch_obj; float yaw_obj; float alt_obj; float vel_obj; //対地速度 float dT_obj; float path_obj; float vdot_obj; Vector3f destination; //誘導地点NED座標 Vector3f turn_center; //旋回中心NED座標 float turn_r; //旋回半径 void limit_obj(); float p_control(float diff, float kp); //比例制御(diff:目標値-現在値, kp:比例ゲイン) float angdiff_pi(float rad); //角度を[-pi, pi]の範囲で出力 float atan_angdiff(float a1,float a2); //atan2を用いた角度差計算 float deg2rad(float deg); //degからradに変換 float calc_path(); //経路角算出 public: Autopilot(); void set_dest(float x, float y); //誘導地点の設定 void set_turn(float x, float y, float r); //定点旋回の設定 void set_alt(float alt, float vel); //高度,速度の設定 void set_climb(float path_angle, float vdot); //経路角,加速度の設定 void update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel, const Vector3f v_dot); //姿勢角等の更新 void level(); //水平飛行 void guide(); //定点誘導 void turn(); //定点旋回(時計回り) void keep_alt(); //高度,速度維持 void climb(); //経路角一定 void return_val(float &r_obj, float &p_obj, float &t_obj); //目標値を代入 }; #endif