File content as of revision 15:28f480b41553:

#ifndef __AUTOPILOT_HPP__
#define __AUTOPILOT_HPP__

#include "mbed.h"
#include "Vector3.hpp"
#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