HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@4:1117ad251fef, 2021-11-15 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 15 13:41:51 2021 +0000
Revision:
4:1117ad251fef
Parent:
3:46d4a32011fc
Child:
6:1f5e6efff5b4
magbias

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 3:46d4a32011fc 7 void Autopilot::update_val(const Vector3 rpy, const float altitude, const Vector3 pos)
osaka 0:e9cdfc6579a7 8 {
osaka 0:e9cdfc6579a7 9 this->roll = rpy.x;
osaka 0:e9cdfc6579a7 10 this->pitch = rpy.y;
osaka 0:e9cdfc6579a7 11 this->yaw = rpy.z;
osaka 0:e9cdfc6579a7 12 this->alt = altitude;
osaka 0:e9cdfc6579a7 13 this->pos_ned = pos;
osaka 0:e9cdfc6579a7 14 }
osaka 0:e9cdfc6579a7 15
osaka 0:e9cdfc6579a7 16 void Autopilot::level()
osaka 0:e9cdfc6579a7 17 {
osaka 0:e9cdfc6579a7 18 roll_obj = 0.0f;
osaka 0:e9cdfc6579a7 19 }
osaka 0:e9cdfc6579a7 20
osaka 0:e9cdfc6579a7 21 void Autopilot::guide()
osaka 0:e9cdfc6579a7 22 {
osaka 0:e9cdfc6579a7 23 float x_dist = destination.x - pos_ned.x;
osaka 0:e9cdfc6579a7 24 float y_dist = destination.y - pos_ned.y;
osaka 0:e9cdfc6579a7 25 yaw_obj = std::atan2(y_dist, x_dist); //rad
osaka 0:e9cdfc6579a7 26 float yaw_dif = angdif_pi(yaw_obj - yaw);
osaka 0:e9cdfc6579a7 27 roll_obj = p_control(yaw_dif, 0.3f);
osaka 0:e9cdfc6579a7 28 }
osaka 0:e9cdfc6579a7 29
osaka 0:e9cdfc6579a7 30 void Autopilot::turn()
osaka 0:e9cdfc6579a7 31 {
osaka 0:e9cdfc6579a7 32 float x_dist = turn_center.x - pos_ned.x;
osaka 0:e9cdfc6579a7 33 float y_dist = turn_center.y - pos_ned.y;
osaka 1:73704460a8b4 34 float yaw_center = std::atan2(y_dist, x_dist);
osaka 0:e9cdfc6579a7 35 float yaw_dif = angdif_pi(yaw_center - yaw); //旋回中心までの方位角, rad
osaka 0:e9cdfc6579a7 36 float dist = std::sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離
osaka 0:e9cdfc6579a7 37 //旋回円から遠い場合は旋回中心まで誘導
osaka 0:e9cdfc6579a7 38 if (dist > 2*turn_r)
osaka 0:e9cdfc6579a7 39 {
osaka 2:049e057f4625 40 set_dest(turn_center.x, turn_center.y);
osaka 0:e9cdfc6579a7 41 guide();
osaka 0:e9cdfc6579a7 42 }
osaka 0:e9cdfc6579a7 43 else
osaka 0:e9cdfc6579a7 44 {
osaka 0:e9cdfc6579a7 45 roll_obj = 0.35f; //基準
NaotoMorita 4:1117ad251fef 46 roll_obj -= 0.6f * (angdif_pi(0.5f*M_PI - yaw_dif)); //旋回中心方向に対する角度を90[deg]にする
osaka 1:73704460a8b4 47 if (std::abs(0.5f*M_PI - yaw_dif) < deg2rad(5)) //距離を保つ
osaka 0:e9cdfc6579a7 48 roll_obj -= 0.035f * (turn_r - dist);
osaka 0:e9cdfc6579a7 49 else
osaka 0:e9cdfc6579a7 50 roll_obj -= 0.01f * (turn_r - dist); //角度のずれが大きいときは角度合わせを優先
osaka 0:e9cdfc6579a7 51 }
osaka 0:e9cdfc6579a7 52 }
osaka 0:e9cdfc6579a7 53
osaka 0:e9cdfc6579a7 54 void Autopilot::keep_alt()
osaka 0:e9cdfc6579a7 55 {
osaka 0:e9cdfc6579a7 56
osaka 0:e9cdfc6579a7 57 }
osaka 0:e9cdfc6579a7 58
osaka 1:73704460a8b4 59 void Autopilot::set_dest(float x, float y)
osaka 0:e9cdfc6579a7 60 {
osaka 1:73704460a8b4 61 this->destination.x = x;
osaka 1:73704460a8b4 62 this->destination.y = y;
osaka 0:e9cdfc6579a7 63 }
osaka 0:e9cdfc6579a7 64
osaka 1:73704460a8b4 65 void Autopilot::set_turn(float x, float y, float r)
osaka 0:e9cdfc6579a7 66 {
osaka 1:73704460a8b4 67 this->turn_center.x = x;
osaka 1:73704460a8b4 68 this->turn_center.y = y;
osaka 0:e9cdfc6579a7 69 this->turn_r = r;
osaka 0:e9cdfc6579a7 70 }
osaka 0:e9cdfc6579a7 71
osaka 2:049e057f4625 72 void Autopilot::return_val(float &r_obj, float &p_obj, float &a_obj)
osaka 0:e9cdfc6579a7 73 {
osaka 0:e9cdfc6579a7 74 limit_obj();
osaka 2:049e057f4625 75 r_obj = this->roll_obj;
osaka 2:049e057f4625 76 p_obj = this->pitch_obj;
osaka 2:049e057f4625 77 a_obj = this->alt_obj;
osaka 0:e9cdfc6579a7 78 }
osaka 0:e9cdfc6579a7 79
osaka 0:e9cdfc6579a7 80 void Autopilot::limit_obj()
osaka 0:e9cdfc6579a7 81 {
osaka 0:e9cdfc6579a7 82 //roll目標値を±30[deg]に制限
osaka 0:e9cdfc6579a7 83 if (roll_obj > deg2rad(30.0f))
osaka 1:73704460a8b4 84 roll_obj = deg2rad(30.0f);
osaka 0:e9cdfc6579a7 85 else if (roll_obj < deg2rad(-30.0f))
osaka 0:e9cdfc6579a7 86 roll_obj = deg2rad(-30.0f);
osaka 0:e9cdfc6579a7 87
osaka 0:e9cdfc6579a7 88 //pitch目標値を±10[deg]に制限
osaka 0:e9cdfc6579a7 89 if (pitch_obj > deg2rad(10.0f))
osaka 0:e9cdfc6579a7 90 pitch_obj = deg2rad(10.0f);
osaka 0:e9cdfc6579a7 91 else if (pitch_obj < deg2rad(-10.0f))
osaka 0:e9cdfc6579a7 92 pitch_obj = deg2rad(-10.0f);
osaka 0:e9cdfc6579a7 93 }
osaka 0:e9cdfc6579a7 94
osaka 0:e9cdfc6579a7 95 float Autopilot::p_control(float dif, float kp)
osaka 0:e9cdfc6579a7 96 {
osaka 0:e9cdfc6579a7 97 return kp * dif;
osaka 0:e9cdfc6579a7 98 }
osaka 0:e9cdfc6579a7 99
osaka 0:e9cdfc6579a7 100 float Autopilot::angdif_pi(float rad)
osaka 0:e9cdfc6579a7 101 {
osaka 1:73704460a8b4 102 if (rad > M_PI)
osaka 1:73704460a8b4 103 return rad - 2*M_PI;
osaka 1:73704460a8b4 104 else if (rad < -M_PI)
osaka 1:73704460a8b4 105 return rad + 2*M_PI;
osaka 0:e9cdfc6579a7 106 else
osaka 0:e9cdfc6579a7 107 return rad;
osaka 0:e9cdfc6579a7 108 }
osaka 0:e9cdfc6579a7 109
osaka 0:e9cdfc6579a7 110 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 111 {
osaka 1:73704460a8b4 112 return deg * M_PI / 180.0f;
osaka 0:e9cdfc6579a7 113 }