Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Diff: Autopilot.hpp
- Revision:
- 0:e9cdfc6579a7
- Child:
- 1:73704460a8b4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Autopilot.hpp Fri Nov 12 06:50:45 2021 +0000 @@ -0,0 +1,52 @@ +#ifndef __AUTOPILOT_HPP__ +#define __AUTOPILOT_HPP__ + +#include "mbed.h" +#include "Vector3.hpp" +#include <vector> +#include <cmath> + +#define 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(Vector3 dest); //誘導地点の設定 + void set_turn(Vector3 center, float r); //定点旋回の設定 + + std::vector<float> return_val(); //目標値を返す +} + +#endif \ No newline at end of file