HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@11:51a0fedd7745, 2021-11-29 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 29 09:45:32 2021 +0000
Revision:
11:51a0fedd7745
Parent:
10:c8b1ab9841c9
Child:
12:59e547742cd8
yaw obj mod

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