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?
User | Revision | Line number | New 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 | } |