HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@9:6c3e1d574f86, 2021-11-19 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 19 09:21:41 2021 +0000
Revision:
9:6c3e1d574f86
Parent:
7:988905aed916
Child:
10:c8b1ab9841c9
heading fuse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
osaka 0:e9cdfc6579a7 1 #include "Autopilot.hpp"
osaka 0:e9cdfc6579a7 2
osaka 0:e9cdfc6579a7 3 Autopilot::Autopilot()
osaka 0:e9cdfc6579a7 4 {
osaka 0:e9cdfc6579a7 5 }
osaka 0:e9cdfc6579a7 6
osaka 1:73704460a8b4 7 void Autopilot::set_dest(float x, float y)
osaka 0:e9cdfc6579a7 8 {
osaka 1:73704460a8b4 9 this->destination.x = x;
osaka 1:73704460a8b4 10 this->destination.y = y;
osaka 0:e9cdfc6579a7 11 }
osaka 0:e9cdfc6579a7 12
osaka 1:73704460a8b4 13 void Autopilot::set_turn(float x, float y, float r)
osaka 0:e9cdfc6579a7 14 {
osaka 1:73704460a8b4 15 this->turn_center.x = x;
osaka 1:73704460a8b4 16 this->turn_center.y = y;
osaka 0:e9cdfc6579a7 17 this->turn_r = r;
osaka 0:e9cdfc6579a7 18 }
osaka 0:e9cdfc6579a7 19
osaka 7:988905aed916 20 void Autopilot::set_alt(float alt, float vel)
osaka 6:1f5e6efff5b4 21 {
osaka 6:1f5e6efff5b4 22 this->alt_obj = alt;
osaka 6:1f5e6efff5b4 23 this->vel_obj = vel;
osaka 6:1f5e6efff5b4 24 }
osaka 6:1f5e6efff5b4 25
osaka 6:1f5e6efff5b4 26 void Autopilot::update_val(const Vector3 rpy, const float altitude, const Vector3 pos, const Vector3 vel)
osaka 6:1f5e6efff5b4 27 {
osaka 6:1f5e6efff5b4 28 this->roll = rpy.x;
osaka 6:1f5e6efff5b4 29 this->pitch = rpy.y;
osaka 6:1f5e6efff5b4 30 this->yaw = rpy.z;
osaka 6:1f5e6efff5b4 31 this->alt = altitude;
osaka 6:1f5e6efff5b4 32 this->pos_ned = pos;
osaka 6:1f5e6efff5b4 33 this->vel_ned = vel;
osaka 6:1f5e6efff5b4 34 }
osaka 6:1f5e6efff5b4 35
osaka 6:1f5e6efff5b4 36 void Autopilot::level()
osaka 6:1f5e6efff5b4 37 {
osaka 6:1f5e6efff5b4 38 roll_obj = 0.0f;
osaka 6:1f5e6efff5b4 39 }
osaka 6:1f5e6efff5b4 40
osaka 6:1f5e6efff5b4 41 void Autopilot::guide()
osaka 6:1f5e6efff5b4 42 {
osaka 6:1f5e6efff5b4 43 float x_dist = destination.x - pos_ned.x;
osaka 6:1f5e6efff5b4 44 float y_dist = destination.y - pos_ned.y;
osaka 6:1f5e6efff5b4 45 yaw_obj = std::atan2(y_dist, x_dist); //rad
osaka 6:1f5e6efff5b4 46 float yaw_diff = angdiff_pi(yaw_obj - yaw);
osaka 6:1f5e6efff5b4 47 roll_obj = p_control(yaw_diff, 0.3f);
osaka 6:1f5e6efff5b4 48 }
osaka 6:1f5e6efff5b4 49
osaka 6:1f5e6efff5b4 50 void Autopilot::turn()
osaka 6:1f5e6efff5b4 51 {
osaka 6:1f5e6efff5b4 52 float x_dist = turn_center.x - pos_ned.x;
osaka 6:1f5e6efff5b4 53 float y_dist = turn_center.y - pos_ned.y;
osaka 6:1f5e6efff5b4 54 float yaw_center = std::atan2(y_dist, x_dist);
osaka 6:1f5e6efff5b4 55 float yaw_diff = angdiff_pi(yaw_center - yaw); //旋回中心までの方位角, rad
osaka 6:1f5e6efff5b4 56 float dist = std::sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離
osaka 6:1f5e6efff5b4 57 //旋回円から遠い場合は旋回中心まで誘導
osaka 6:1f5e6efff5b4 58 if (dist > 2*turn_r)
osaka 6:1f5e6efff5b4 59 {
osaka 6:1f5e6efff5b4 60 set_dest(turn_center.x, turn_center.y);
osaka 6:1f5e6efff5b4 61 guide();
osaka 6:1f5e6efff5b4 62 }
osaka 6:1f5e6efff5b4 63 else
osaka 6:1f5e6efff5b4 64 {
osaka 6:1f5e6efff5b4 65 roll_obj = 0.35f; //基準
osaka 6:1f5e6efff5b4 66 roll_obj -= 0.6f * (angdiff_pi(0.5f*M_PI - yaw_diff)); //旋回中心方向に対する角度を90[deg]にする
osaka 6:1f5e6efff5b4 67 if (std::abs(0.5f*M_PI - yaw_diff) < deg2rad(5)) //距離を保つ
osaka 6:1f5e6efff5b4 68 roll_obj -= 0.035f * (turn_r - dist);
osaka 6:1f5e6efff5b4 69 else
osaka 6:1f5e6efff5b4 70 roll_obj -= 0.01f * (turn_r - dist); //角度のずれが大きいときは角度合わせを優先
osaka 6:1f5e6efff5b4 71 }
osaka 6:1f5e6efff5b4 72 }
osaka 6:1f5e6efff5b4 73
osaka 6:1f5e6efff5b4 74 void Autopilot::keep_alt()
osaka 6:1f5e6efff5b4 75 {
osaka 6:1f5e6efff5b4 76 //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
osaka 6:1f5e6efff5b4 77 float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z;
osaka 6:1f5e6efff5b4 78 float E = alt + v2 / (2*G);
osaka 6:1f5e6efff5b4 79 float Esp = alt_obj + vel_obj*vel_obj / (2*G);
NaotoMorita 9:6c3e1d574f86 80 float dT_cruise = 0.0f;
osaka 6:1f5e6efff5b4 81 dT_obj = dT_cruise;
NaotoMorita 9:6c3e1d574f86 82 dT_obj += p_control(Esp - E, 1.0f);
osaka 6:1f5e6efff5b4 83
osaka 6:1f5e6efff5b4 84 float B = alt - v2 / (2*G);
osaka 6:1f5e6efff5b4 85 float Bsp = alt_obj - vel_obj*vel_obj / (2*G);
osaka 6:1f5e6efff5b4 86 pitch_obj = p_control(Bsp - B, 0.02f);
osaka 6:1f5e6efff5b4 87 }
osaka 6:1f5e6efff5b4 88
osaka 7:988905aed916 89 void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj)
osaka 0:e9cdfc6579a7 90 {
osaka 0:e9cdfc6579a7 91 limit_obj();
osaka 2:049e057f4625 92 r_obj = this->roll_obj;
osaka 2:049e057f4625 93 p_obj = this->pitch_obj;
osaka 6:1f5e6efff5b4 94 t_obj = this->dT_obj;
osaka 0:e9cdfc6579a7 95 }
osaka 0:e9cdfc6579a7 96
osaka 0:e9cdfc6579a7 97 void Autopilot::limit_obj()
osaka 0:e9cdfc6579a7 98 {
osaka 0:e9cdfc6579a7 99 //roll目標値を±30[deg]に制限
osaka 0:e9cdfc6579a7 100 if (roll_obj > deg2rad(30.0f))
osaka 1:73704460a8b4 101 roll_obj = deg2rad(30.0f);
osaka 0:e9cdfc6579a7 102 else if (roll_obj < deg2rad(-30.0f))
osaka 0:e9cdfc6579a7 103 roll_obj = deg2rad(-30.0f);
osaka 0:e9cdfc6579a7 104
osaka 0:e9cdfc6579a7 105 //pitch目標値を±10[deg]に制限
osaka 0:e9cdfc6579a7 106 if (pitch_obj > deg2rad(10.0f))
osaka 0:e9cdfc6579a7 107 pitch_obj = deg2rad(10.0f);
osaka 0:e9cdfc6579a7 108 else if (pitch_obj < deg2rad(-10.0f))
osaka 0:e9cdfc6579a7 109 pitch_obj = deg2rad(-10.0f);
osaka 6:1f5e6efff5b4 110
osaka 6:1f5e6efff5b4 111 //dT目標値を0~1に制限
osaka 6:1f5e6efff5b4 112 if (dT_obj > 1.0f)
osaka 6:1f5e6efff5b4 113 dT_obj = 1.0f;
NaotoMorita 9:6c3e1d574f86 114 else if (dT_obj < -1.0f)
NaotoMorita 9:6c3e1d574f86 115 dT_obj = -1.0f;
osaka 0:e9cdfc6579a7 116 }
osaka 0:e9cdfc6579a7 117
osaka 6:1f5e6efff5b4 118 float Autopilot::p_control(float diff, float kp)
osaka 0:e9cdfc6579a7 119 {
osaka 6:1f5e6efff5b4 120 return kp * diff;
osaka 0:e9cdfc6579a7 121 }
osaka 0:e9cdfc6579a7 122
osaka 6:1f5e6efff5b4 123 float Autopilot::angdiff_pi(float rad)
osaka 0:e9cdfc6579a7 124 {
osaka 1:73704460a8b4 125 if (rad > M_PI)
osaka 1:73704460a8b4 126 return rad - 2*M_PI;
osaka 1:73704460a8b4 127 else if (rad < -M_PI)
osaka 1:73704460a8b4 128 return rad + 2*M_PI;
osaka 0:e9cdfc6579a7 129 else
osaka 0:e9cdfc6579a7 130 return rad;
osaka 0:e9cdfc6579a7 131 }
osaka 0:e9cdfc6579a7 132
osaka 0:e9cdfc6579a7 133 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 134 {
osaka 1:73704460a8b4 135 return deg * M_PI / 180.0f;
osaka 0:e9cdfc6579a7 136 }