File content as of revision 3:46d4a32011fc:

#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(const Vector3 rpy, const float altitude, const 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