File content as of revision 19:86ed9a546684:

#include "Autopilot.hpp"

Autopilot::Autopilot()
{
}

void Autopilot::set_dest(float x, float y)
{
    this->destination(0) = x;
    this->destination(1) = y;
}

void Autopilot::set_turn(float x, float y, float r)
{
    this->turn_center(0) = x;
    this->turn_center(1) = y;
    this->turn_r = r;
}

void Autopilot::set_alt(float alt, float vel)
{
    this->alt_obj = alt;
    this->vel_obj = vel;
}

//void Autopilot::set_climb(float path_ang, float vdot)
//{
//    this->path_obj = path_ang;
//    this->vdot_obj = vdot;
//}

void Autopilot::update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel)
{
    this->roll = rpy(0);
    this->pitch = rpy(1);
    this->yaw = rpy(2);
    this->alt_before = this->alt;
    this->alt = altitude;
    this->pos_before = this->pos_ned;
    this->pos_ned(0) = pos(0);
    this->pos_ned(1) = pos(1);
    this->pos_ned(2) = pos(2);
    this->vel_before = this->vel_ned;
    this->vel_ned(0) = vel(0);
    this->vel_ned(1) = vel(1);
    this->vel_ned(2) = vel(2);
    //this->vdot = v_dot;
}

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

void Autopilot::guide()
{
    float x_dist = destination(0) - pos_ned(0);
    float y_dist = destination(1) - pos_ned(1);
    yaw_obj = std::atan2(y_dist, x_dist);   //rad
    float yaw_diff = atan_angdiff(yaw_obj,yaw);
    roll_obj = p_control(yaw_diff, 0.3f);
}

void Autopilot::turn()
{
    float x_dist = turn_center(0) - pos_ned(0);
    float y_dist = turn_center(1) - pos_ned(1);
    float yaw_center = std::atan2(y_dist, x_dist);
    float yaw_diff = atan_angdiff(yaw_center,yaw);    //旋回中心までの方位角, rad
    float dist = sqrt(x_dist*x_dist + y_dist*y_dist);    //旋回中心までの距離
    //旋回円から遠い場合は旋回中心まで誘導
    if (dist > 2*turn_r)
    {
        set_dest(turn_center(0), turn_center(1));
        guide();
    }
    else
    {
        roll_obj = 0.35f;   //基準
        float yaw_obj = 0.5f*M_PI_F+0.05f*(turn_r - dist);
        float coasediff = (atan_angdiff(yaw_obj,yaw_diff));
        roll_obj -= 0.4f * coasediff;     //旋回中心方向に対する角度を90[deg]にする
    }
}

void Autopilot::keep_alt()
{
    //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
    float v2 = vel_ned(0)*vel_ned(0) + vel_ned(1)*vel_ned(1) + vel_ned(2)*vel_ned(2);
    float E = alt + v2 / (2*G);
    float alt_sp;
    if (alt_obj - alt > 5.0f)
        alt_sp = alt + 5.0f;
    else if (alt_obj - alt < -5.0f)
        alt_sp = alt - 5.0f;
    else
        alt_sp = alt_obj;
    float Esp = alt_sp + vel_obj*vel_obj / (2*G);
    float dT_cruise = 0.0f;
    dT_obj = dT_cruise;
    dT_obj += p_control(Esp - E, 0.1f);
    
    float B = alt - v2 / (2*G);
    float Bsp = alt_sp - vel_obj*vel_obj / (2*G);
    pitch_obj = p_control(Bsp - B, 0.05f);
}

//void Autopilot::climb()
//{
 //   //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
 //   float Edot = path_ang + vdot.norm() / G;
 //   float Edot_sp = path_obj + vdot_obj / G;
 //   float dT_cruise = 0.0f;
 //   dT_obj = dT_cruise;
 //   dT_obj += p_control(Edot_sp - Edot, 10.0f);
    
 //   float Bdot = path_ang - vdot.norm() / G;
 //   float Bdot_sp = path_obj - vdot_obj / G;
 //   pitch_obj = p_control(Bdot_sp - Bdot, 10.0f);
//}

void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj)
{
    limit_obj();
    r_obj = this->roll_obj;
    p_obj = this->pitch_obj;
    t_obj = this->dT_obj;
}

void Autopilot::limit_obj()
{
    //roll目標値を±30[deg]に制限
    if (roll_obj > deg2rad(30.0f))
        roll_obj = 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);
        
    //dT目標値を0~1に制限
    if (dT_obj > 1.0f)
        dT_obj = 1.0f;
    else if (dT_obj < -1.0f)
        dT_obj = -1.0f;
}

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

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

float Autopilot::atan_angdiff(float a1,float a2)
{
    return std::atan2(std::sin(a1-a2),std::cos(a1-a2));
}

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

//float Autopilot::calc_path()
//{
//    float alt_diff = alt - alt_before;
//    float xy_diff = sqrt((pos_ned(0) - pos_before(0))*(pos_ned(0) - pos_before(0)) + (pos_ned(1) - pos_before(1))*(pos_ned(1) - pos_before(1)));
//    path_ang = std::atan2(alt_diff, xy_diff);
//}