Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.cpp@13:75636841e43b, 2021-12-01 (annotated)
- Committer:
- osaka
- Date:
- Wed Dec 01 09:23:26 2021 +0000
- Revision:
- 13:75636841e43b
- Parent:
- 12:59e547742cd8
- Child:
- 14:7b6c3f8b85fb
limit thrust
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 | 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 | 12:59e547742cd8 | 26 | void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix 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 | 12:59e547742cd8 | 32 | this->pos_ned.x = pos(1, 1); |
osaka | 12:59e547742cd8 | 33 | this->pos_ned.y = pos(2, 1); |
osaka | 12:59e547742cd8 | 34 | this->pos_ned.z = pos(3, 1); |
osaka | 12:59e547742cd8 | 35 | this->vel_ned.x = vel(1, 1); |
osaka | 12:59e547742cd8 | 36 | this->vel_ned.y = vel(2, 1); |
osaka | 12:59e547742cd8 | 37 | this->vel_ned.z = vel(3, 1); |
osaka | 6:1f5e6efff5b4 | 38 | } |
osaka | 6:1f5e6efff5b4 | 39 | |
osaka | 6:1f5e6efff5b4 | 40 | void Autopilot::level() |
osaka | 6:1f5e6efff5b4 | 41 | { |
osaka | 6:1f5e6efff5b4 | 42 | roll_obj = 0.0f; |
osaka | 6:1f5e6efff5b4 | 43 | } |
osaka | 6:1f5e6efff5b4 | 44 | |
osaka | 6:1f5e6efff5b4 | 45 | void Autopilot::guide() |
osaka | 6:1f5e6efff5b4 | 46 | { |
osaka | 6:1f5e6efff5b4 | 47 | float x_dist = destination.x - pos_ned.x; |
osaka | 6:1f5e6efff5b4 | 48 | float y_dist = destination.y - pos_ned.y; |
NaotoMorita | 11:51a0fedd7745 | 49 | yaw_obj = atan2f(y_dist, x_dist); //rad |
NaotoMorita | 11:51a0fedd7745 | 50 | float yaw_diff = atan_angdiff(yaw_obj,yaw); |
osaka | 6:1f5e6efff5b4 | 51 | roll_obj = p_control(yaw_diff, 0.3f); |
osaka | 6:1f5e6efff5b4 | 52 | } |
osaka | 6:1f5e6efff5b4 | 53 | |
osaka | 6:1f5e6efff5b4 | 54 | void Autopilot::turn() |
osaka | 6:1f5e6efff5b4 | 55 | { |
osaka | 6:1f5e6efff5b4 | 56 | float x_dist = turn_center.x - pos_ned.x; |
osaka | 6:1f5e6efff5b4 | 57 | float y_dist = turn_center.y - pos_ned.y; |
NaotoMorita | 11:51a0fedd7745 | 58 | float yaw_center = atan2f(y_dist, x_dist); |
NaotoMorita | 11:51a0fedd7745 | 59 | float yaw_diff = atan_angdiff(yaw_center,yaw); //旋回中心までの方位角, rad |
NaotoMorita | 11:51a0fedd7745 | 60 | float dist = sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離 |
osaka | 6:1f5e6efff5b4 | 61 | //旋回円から遠い場合は旋回中心まで誘導 |
osaka | 6:1f5e6efff5b4 | 62 | if (dist > 2*turn_r) |
osaka | 6:1f5e6efff5b4 | 63 | { |
osaka | 6:1f5e6efff5b4 | 64 | set_dest(turn_center.x, turn_center.y); |
osaka | 6:1f5e6efff5b4 | 65 | guide(); |
osaka | 6:1f5e6efff5b4 | 66 | } |
osaka | 6:1f5e6efff5b4 | 67 | else |
osaka | 6:1f5e6efff5b4 | 68 | { |
osaka | 6:1f5e6efff5b4 | 69 | roll_obj = 0.35f; //基準 |
NaotoMorita | 11:51a0fedd7745 | 70 | float yaw_obj = 0.5f*M_PI+0.05f*(turn_r - dist); |
NaotoMorita | 11:51a0fedd7745 | 71 | float coasediff = (atan_angdiff(yaw_obj,yaw_diff)); |
NaotoMorita | 11:51a0fedd7745 | 72 | roll_obj -= 0.4f * coasediff; //旋回中心方向に対する角度を90[deg]にする |
osaka | 6:1f5e6efff5b4 | 73 | } |
osaka | 6:1f5e6efff5b4 | 74 | } |
osaka | 6:1f5e6efff5b4 | 75 | |
osaka | 6:1f5e6efff5b4 | 76 | void Autopilot::keep_alt() |
osaka | 6:1f5e6efff5b4 | 77 | { |
osaka | 6:1f5e6efff5b4 | 78 | //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html), p制御のみ |
osaka | 6:1f5e6efff5b4 | 79 | float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z; |
osaka | 6:1f5e6efff5b4 | 80 | float E = alt + v2 / (2*G); |
osaka | 13:75636841e43b | 81 | float alt_sp; |
osaka | 13:75636841e43b | 82 | if (alt_obj - alt > 5.0f) |
osaka | 13:75636841e43b | 83 | alt_sp = alt + 5.0f; |
osaka | 13:75636841e43b | 84 | else if (alt_obj - alt < -5.0f) |
osaka | 13:75636841e43b | 85 | alt_sp = alt - 5.0f; |
osaka | 13:75636841e43b | 86 | else |
osaka | 13:75636841e43b | 87 | alt_sp = alt_obj; |
osaka | 13:75636841e43b | 88 | float Esp = alt_sp + vel_obj*vel_obj / (2*G); |
NaotoMorita | 9:6c3e1d574f86 | 89 | float dT_cruise = 0.0f; |
osaka | 6:1f5e6efff5b4 | 90 | dT_obj = dT_cruise; |
NaotoMorita | 11:51a0fedd7745 | 91 | dT_obj += p_control(Esp - E, 0.1f); |
osaka | 6:1f5e6efff5b4 | 92 | |
osaka | 6:1f5e6efff5b4 | 93 | float B = alt - v2 / (2*G); |
osaka | 13:75636841e43b | 94 | float Bsp = alt_sp - vel_obj*vel_obj / (2*G); |
NaotoMorita | 11:51a0fedd7745 | 95 | pitch_obj = p_control(Bsp - B, 0.05f); |
osaka | 6:1f5e6efff5b4 | 96 | } |
osaka | 6:1f5e6efff5b4 | 97 | |
osaka | 7:988905aed916 | 98 | void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj) |
osaka | 0:e9cdfc6579a7 | 99 | { |
osaka | 0:e9cdfc6579a7 | 100 | limit_obj(); |
osaka | 2:049e057f4625 | 101 | r_obj = this->roll_obj; |
osaka | 2:049e057f4625 | 102 | p_obj = this->pitch_obj; |
osaka | 6:1f5e6efff5b4 | 103 | t_obj = this->dT_obj; |
osaka | 0:e9cdfc6579a7 | 104 | } |
osaka | 0:e9cdfc6579a7 | 105 | |
osaka | 0:e9cdfc6579a7 | 106 | void Autopilot::limit_obj() |
osaka | 0:e9cdfc6579a7 | 107 | { |
osaka | 0:e9cdfc6579a7 | 108 | //roll目標値を±30[deg]に制限 |
osaka | 0:e9cdfc6579a7 | 109 | if (roll_obj > deg2rad(30.0f)) |
osaka | 1:73704460a8b4 | 110 | roll_obj = deg2rad(30.0f); |
osaka | 0:e9cdfc6579a7 | 111 | else if (roll_obj < deg2rad(-30.0f)) |
osaka | 0:e9cdfc6579a7 | 112 | roll_obj = deg2rad(-30.0f); |
osaka | 0:e9cdfc6579a7 | 113 | |
osaka | 0:e9cdfc6579a7 | 114 | //pitch目標値を±10[deg]に制限 |
osaka | 0:e9cdfc6579a7 | 115 | if (pitch_obj > deg2rad(10.0f)) |
osaka | 0:e9cdfc6579a7 | 116 | pitch_obj = deg2rad(10.0f); |
osaka | 0:e9cdfc6579a7 | 117 | else if (pitch_obj < deg2rad(-10.0f)) |
osaka | 0:e9cdfc6579a7 | 118 | pitch_obj = deg2rad(-10.0f); |
osaka | 6:1f5e6efff5b4 | 119 | |
osaka | 6:1f5e6efff5b4 | 120 | //dT目標値を0~1に制限 |
osaka | 6:1f5e6efff5b4 | 121 | if (dT_obj > 1.0f) |
osaka | 6:1f5e6efff5b4 | 122 | dT_obj = 1.0f; |
NaotoMorita | 9:6c3e1d574f86 | 123 | else if (dT_obj < -1.0f) |
NaotoMorita | 9:6c3e1d574f86 | 124 | dT_obj = -1.0f; |
osaka | 0:e9cdfc6579a7 | 125 | } |
osaka | 0:e9cdfc6579a7 | 126 | |
osaka | 6:1f5e6efff5b4 | 127 | float Autopilot::p_control(float diff, float kp) |
osaka | 0:e9cdfc6579a7 | 128 | { |
osaka | 6:1f5e6efff5b4 | 129 | return kp * diff; |
osaka | 0:e9cdfc6579a7 | 130 | } |
osaka | 0:e9cdfc6579a7 | 131 | |
osaka | 6:1f5e6efff5b4 | 132 | float Autopilot::angdiff_pi(float rad) |
osaka | 0:e9cdfc6579a7 | 133 | { |
osaka | 1:73704460a8b4 | 134 | if (rad > M_PI) |
osaka | 1:73704460a8b4 | 135 | return rad - 2*M_PI; |
osaka | 1:73704460a8b4 | 136 | else if (rad < -M_PI) |
osaka | 1:73704460a8b4 | 137 | return rad + 2*M_PI; |
osaka | 0:e9cdfc6579a7 | 138 | else |
osaka | 0:e9cdfc6579a7 | 139 | return rad; |
osaka | 0:e9cdfc6579a7 | 140 | } |
osaka | 0:e9cdfc6579a7 | 141 | |
NaotoMorita | 11:51a0fedd7745 | 142 | float Autopilot::atan_angdiff(float a1,float a2) |
NaotoMorita | 11:51a0fedd7745 | 143 | { |
NaotoMorita | 11:51a0fedd7745 | 144 | return atan2f(sinf(a1-a2),cosf(a1-a2)); |
NaotoMorita | 11:51a0fedd7745 | 145 | } |
NaotoMorita | 11:51a0fedd7745 | 146 | |
osaka | 0:e9cdfc6579a7 | 147 | float Autopilot::deg2rad(float deg) |
osaka | 0:e9cdfc6579a7 | 148 | { |
osaka | 1:73704460a8b4 | 149 | return deg * M_PI / 180.0f; |
osaka | 0:e9cdfc6579a7 | 150 | } |