HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@1:73704460a8b4, 2021-11-12 (annotated)

Committer:
osaka
Date:
Fri Nov 12 09:03:10 2021 +0000
Revision:
1:73704460a8b4
Parent:
0:e9cdfc6579a7
Child:
2:049e057f4625
debug

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::update_val(Vector3 rpy, float altitude, 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 //旋回円から遠い場合は旋回中心まで誘導
osaka 0:e9cdfc6579a7 39 if (dist > 2*turn_r)
osaka 0:e9cdfc6579a7 40 {
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; //基準
osaka 1:73704460a8b4 46 roll_obj -= 0.6f * (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 0:e9cdfc6579a7 72 std::vector<float> Autopilot::return_val()
osaka 0:e9cdfc6579a7 73 {
osaka 0:e9cdfc6579a7 74 limit_obj();
osaka 1:73704460a8b4 75 std::vector<float> obj;
osaka 1:73704460a8b4 76 obj[0] = roll_obj;
osaka 1:73704460a8b4 77 obj[1] = pitch_obj;
osaka 1:73704460a8b4 78 obj[2] = alt_obj;
osaka 1:73704460a8b4 79 return obj;
osaka 0:e9cdfc6579a7 80 }
osaka 0:e9cdfc6579a7 81
osaka 0:e9cdfc6579a7 82 void Autopilot::limit_obj()
osaka 0:e9cdfc6579a7 83 {
osaka 0:e9cdfc6579a7 84 //roll目標値を±30[deg]に制限
osaka 0:e9cdfc6579a7 85 if (roll_obj > deg2rad(30.0f))
osaka 1:73704460a8b4 86 roll_obj = deg2rad(30.0f);
osaka 0:e9cdfc6579a7 87 else if (roll_obj < deg2rad(-30.0f))
osaka 0:e9cdfc6579a7 88 roll_obj = deg2rad(-30.0f);
osaka 0:e9cdfc6579a7 89
osaka 0:e9cdfc6579a7 90 //pitch目標値を±10[deg]に制限
osaka 0:e9cdfc6579a7 91 if (pitch_obj > deg2rad(10.0f))
osaka 0:e9cdfc6579a7 92 pitch_obj = deg2rad(10.0f);
osaka 0:e9cdfc6579a7 93 else if (pitch_obj < deg2rad(-10.0f))
osaka 0:e9cdfc6579a7 94 pitch_obj = deg2rad(-10.0f);
osaka 0:e9cdfc6579a7 95 }
osaka 0:e9cdfc6579a7 96
osaka 0:e9cdfc6579a7 97 float Autopilot::p_control(float dif, float kp)
osaka 0:e9cdfc6579a7 98 {
osaka 0:e9cdfc6579a7 99 return kp * dif;
osaka 0:e9cdfc6579a7 100 }
osaka 0:e9cdfc6579a7 101
osaka 0:e9cdfc6579a7 102 float Autopilot::angdif_pi(float rad)
osaka 0:e9cdfc6579a7 103 {
osaka 1:73704460a8b4 104 if (rad > M_PI)
osaka 1:73704460a8b4 105 return rad - 2*M_PI;
osaka 1:73704460a8b4 106 else if (rad < -M_PI)
osaka 1:73704460a8b4 107 return rad + 2*M_PI;
osaka 0:e9cdfc6579a7 108 else
osaka 0:e9cdfc6579a7 109 return rad;
osaka 0:e9cdfc6579a7 110 }
osaka 0:e9cdfc6579a7 111
osaka 0:e9cdfc6579a7 112 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 113 {
osaka 1:73704460a8b4 114 return deg * M_PI / 180.0f;
osaka 0:e9cdfc6579a7 115 }