HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@0:e9cdfc6579a7, 2021-11-12 (annotated)

Committer:
osaka
Date:
Fri Nov 12 06:50:45 2021 +0000
Revision:
0:e9cdfc6579a7
Child:
1:73704460a8b4
for autopilot

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 0:e9cdfc6579a7 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 0:e9cdfc6579a7 34 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 0:e9cdfc6579a7 46 roll_obj -= 0.6f * (0.5f*pi - yaw_dif); //旋回中心方向に対する角度を90[deg]にする
osaka 0:e9cdfc6579a7 47 if (std::abs(0.5f*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 0:e9cdfc6579a7 59 void Autopilot::set_dest(Vector3 dest)
osaka 0:e9cdfc6579a7 60 {
osaka 0:e9cdfc6579a7 61 this->destination = dest;
osaka 0:e9cdfc6579a7 62 }
osaka 0:e9cdfc6579a7 63
osaka 0:e9cdfc6579a7 64 void Autopilot::set_turn(Vector3 center, float r)
osaka 0:e9cdfc6579a7 65 {
osaka 0:e9cdfc6579a7 66 this->turn_center = center;
osaka 0:e9cdfc6579a7 67 this->turn_r = r;
osaka 0:e9cdfc6579a7 68 }
osaka 0:e9cdfc6579a7 69
osaka 0:e9cdfc6579a7 70 std::vector<float> Autopilot::return_val()
osaka 0:e9cdfc6579a7 71 {
osaka 0:e9cdfc6579a7 72 limit_obj();
osaka 0:e9cdfc6579a7 73 return {roll_obj, pitch_obj, alt_obj};
osaka 0:e9cdfc6579a7 74 }
osaka 0:e9cdfc6579a7 75
osaka 0:e9cdfc6579a7 76 void Autopilot::limit_obj()
osaka 0:e9cdfc6579a7 77 {
osaka 0:e9cdfc6579a7 78 //roll目標値を±30[deg]に制限
osaka 0:e9cdfc6579a7 79 if (roll_obj > deg2rad(30.0f))
osaka 0:e9cdfc6579a7 80 roll_objj = deg2rad(30.0f);
osaka 0:e9cdfc6579a7 81 else if (roll_obj < deg2rad(-30.0f))
osaka 0:e9cdfc6579a7 82 roll_obj = deg2rad(-30.0f);
osaka 0:e9cdfc6579a7 83
osaka 0:e9cdfc6579a7 84 //pitch目標値を±10[deg]に制限
osaka 0:e9cdfc6579a7 85 if (pitch_obj > deg2rad(10.0f))
osaka 0:e9cdfc6579a7 86 pitch_obj = deg2rad(10.0f);
osaka 0:e9cdfc6579a7 87 else if (pitch_obj < deg2rad(-10.0f))
osaka 0:e9cdfc6579a7 88 pitch_obj = deg2rad(-10.0f);
osaka 0:e9cdfc6579a7 89 }
osaka 0:e9cdfc6579a7 90
osaka 0:e9cdfc6579a7 91 float Autopilot::p_control(float dif, float kp)
osaka 0:e9cdfc6579a7 92 {
osaka 0:e9cdfc6579a7 93 return kp * dif;
osaka 0:e9cdfc6579a7 94 }
osaka 0:e9cdfc6579a7 95
osaka 0:e9cdfc6579a7 96 float Autopilot::angdif_pi(float rad)
osaka 0:e9cdfc6579a7 97 {
osaka 0:e9cdfc6579a7 98 if (rad > pi)
osaka 0:e9cdfc6579a7 99 return rad - 2*pi;
osaka 0:e9cdfc6579a7 100 else if (rad < -pi)
osaka 0:e9cdfc6579a7 101 return rad + 2*pi;
osaka 0:e9cdfc6579a7 102 else
osaka 0:e9cdfc6579a7 103 return rad;
osaka 0:e9cdfc6579a7 104 }
osaka 0:e9cdfc6579a7 105
osaka 0:e9cdfc6579a7 106 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 107 {
osaka 0:e9cdfc6579a7 108 return deg * pi / 180.0f;
osaka 0:e9cdfc6579a7 109 }