File content as of revision 0:e9cdfc6579a7:

#include "Autopilot.hpp"

Autopilot::Autopilot()
{
}

void Autopilot::update_val(Vector3 rpy, float altitude, vector3 pos)
{
    this->roll = rpy.x;
    this->pitch = rpy.y;
    this->yaw = rpy.z;
    this->alt = altitude;
    this->pos_ned = pos;
}

void Autopilot::level()
{
    roll_obj = 0.0f;
}

void Autopilot::guide()
{
    float x_dist = destination.x - pos_ned.x;
    float y_dist = destination.y - pos_ned.y;
    yaw_obj = std::atan2(y_dist, x_dist);   //rad
    float yaw_dif = angdif_pi(yaw_obj - yaw);
    roll_obj = p_control(yaw_dif, 0.3f);
}

void Autopilot::turn()
{
    float x_dist = turn_center.x - pos_ned.x;
    float y_dist = turn_center.y - pos_ned.y;
    yaw_center = std::atan2(y_dist, x_dist);
    float yaw_dif = angdif_pi(yaw_center - yaw);    //旋回中心までの方位角, rad
    float dist = std::sqrt(x_dist*x_dist + y_dist*y_dist);    //旋回中心までの距離
    
    //旋回円から遠い場合は旋回中心まで誘導
    if (dist > 2*turn_r)
    {
        guide();
    }
    else
    {
        roll_obj = 0.35f;   //基準
        roll_obj -= 0.6f * (0.5f*pi - yaw_dif);     //旋回中心方向に対する角度を90[deg]にする
        if (std::abs(0.5f*pi - yaw_dif) < deg2rad(5))   //距離を保つ
            roll_obj -= 0.035f * (turn_r - dist);
        else
            roll_obj -= 0.01f * (turn_r - dist);   //角度のずれが大きいときは角度合わせを優先
    }
}

void Autopilot::keep_alt()
{
    
}

void Autopilot::set_dest(Vector3 dest)
{
    this->destination = dest;
}

void Autopilot::set_turn(Vector3 center, float r)
{
    this->turn_center = center;
    this->turn_r = r;
}

std::vector<float> Autopilot::return_val()
{
    limit_obj();
    return {roll_obj, pitch_obj, alt_obj};
}

void Autopilot::limit_obj()
{
    //roll目標値を±30[deg]に制限
    if (roll_obj > deg2rad(30.0f))
        roll_objj = deg2rad(30.0f);
    else if (roll_obj < deg2rad(-30.0f))
        roll_obj = deg2rad(-30.0f);

    //pitch目標値を±10[deg]に制限
    if (pitch_obj > deg2rad(10.0f))
        pitch_obj = deg2rad(10.0f);
    else if (pitch_obj < deg2rad(-10.0f))
        pitch_obj = deg2rad(-10.0f);
}

float Autopilot::p_control(float dif, float kp)
{
    return kp * dif;
}

float Autopilot::angdif_pi(float rad)
{
    if (rad > pi)   
        return rad - 2*pi;
    else if (rad < -pi)     
        return rad + 2*pi;
    else    
        return rad;
}

float Autopilot::deg2rad(float deg)
{
    return deg * pi / 180.0f;
}