HAPSRG / Autopilot_Eigen

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?

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::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 }