HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@16:36d034dec6d6, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:43:43 2021 +0000
Revision:
16:36d034dec6d6
Parent:
15:28f480b41553
Child:
18:5e2e03a30e1c
std::atan2

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 {
cocorlow 15:28f480b41553 9 this->destination(0) = x;
cocorlow 15:28f480b41553 10 this->destination(1) = 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 {
cocorlow 15:28f480b41553 15 this->turn_center(0) = x;
cocorlow 15:28f480b41553 16 this->turn_center(1) = 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 14:7b6c3f8b85fb 26 void Autopilot::set_climb(float path_ang, float vdot)
osaka 14:7b6c3f8b85fb 27 {
osaka 14:7b6c3f8b85fb 28 this->path_obj = path_ang;
osaka 14:7b6c3f8b85fb 29 this->vdot_obj = vdot;
osaka 14:7b6c3f8b85fb 30 }
osaka 14:7b6c3f8b85fb 31
cocorlow 15:28f480b41553 32 void Autopilot::update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel, const Vector3f v_dot)
osaka 6:1f5e6efff5b4 33 {
cocorlow 15:28f480b41553 34 this->roll = rpy(0);
cocorlow 15:28f480b41553 35 this->pitch = rpy(1);
cocorlow 15:28f480b41553 36 this->yaw = rpy(2);
osaka 14:7b6c3f8b85fb 37 this->alt_before = this->alt;
osaka 6:1f5e6efff5b4 38 this->alt = altitude;
osaka 14:7b6c3f8b85fb 39 this->pos_before = this->pos_ned;
cocorlow 15:28f480b41553 40 this->pos_ned(0) = pos(0);
cocorlow 15:28f480b41553 41 this->pos_ned(1) = pos(1);
cocorlow 15:28f480b41553 42 this->pos_ned(2) = pos(2);
osaka 14:7b6c3f8b85fb 43 this->vel_before = this->vel_ned;
cocorlow 15:28f480b41553 44 this->vel_ned(0) = vel(0);
cocorlow 15:28f480b41553 45 this->vel_ned(1) = vel(1);
cocorlow 15:28f480b41553 46 this->vel_ned(2) = vel(2);
osaka 14:7b6c3f8b85fb 47 this->vdot = v_dot;
osaka 6:1f5e6efff5b4 48 }
osaka 6:1f5e6efff5b4 49
osaka 6:1f5e6efff5b4 50 void Autopilot::level()
osaka 6:1f5e6efff5b4 51 {
osaka 6:1f5e6efff5b4 52 roll_obj = 0.0f;
osaka 6:1f5e6efff5b4 53 }
osaka 6:1f5e6efff5b4 54
osaka 6:1f5e6efff5b4 55 void Autopilot::guide()
osaka 6:1f5e6efff5b4 56 {
cocorlow 15:28f480b41553 57 float x_dist = destination(0) - pos_ned(0);
cocorlow 15:28f480b41553 58 float y_dist = destination(1) - pos_ned(1);
cocorlow 16:36d034dec6d6 59 yaw_obj = std::atan2(y_dist, x_dist); //rad
NaotoMorita 11:51a0fedd7745 60 float yaw_diff = atan_angdiff(yaw_obj,yaw);
osaka 6:1f5e6efff5b4 61 roll_obj = p_control(yaw_diff, 0.3f);
osaka 6:1f5e6efff5b4 62 }
osaka 6:1f5e6efff5b4 63
osaka 6:1f5e6efff5b4 64 void Autopilot::turn()
osaka 6:1f5e6efff5b4 65 {
cocorlow 15:28f480b41553 66 float x_dist = turn_center(0) - pos_ned(0);
cocorlow 15:28f480b41553 67 float y_dist = turn_center(1) - pos_ned(1);
cocorlow 16:36d034dec6d6 68 float yaw_center = std::atan2(y_dist, x_dist);
NaotoMorita 11:51a0fedd7745 69 float yaw_diff = atan_angdiff(yaw_center,yaw); //旋回中心までの方位角, rad
NaotoMorita 11:51a0fedd7745 70 float dist = sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離
osaka 6:1f5e6efff5b4 71 //旋回円から遠い場合は旋回中心まで誘導
osaka 6:1f5e6efff5b4 72 if (dist > 2*turn_r)
osaka 6:1f5e6efff5b4 73 {
cocorlow 15:28f480b41553 74 set_dest(turn_center(0), turn_center(1));
osaka 6:1f5e6efff5b4 75 guide();
osaka 6:1f5e6efff5b4 76 }
osaka 6:1f5e6efff5b4 77 else
osaka 6:1f5e6efff5b4 78 {
osaka 6:1f5e6efff5b4 79 roll_obj = 0.35f; //基準
cocorlow 15:28f480b41553 80 float yaw_obj = 0.5f*M_PI_F+0.05f*(turn_r - dist);
NaotoMorita 11:51a0fedd7745 81 float coasediff = (atan_angdiff(yaw_obj,yaw_diff));
NaotoMorita 11:51a0fedd7745 82 roll_obj -= 0.4f * coasediff; //旋回中心方向に対する角度を90[deg]にする
osaka 6:1f5e6efff5b4 83 }
osaka 6:1f5e6efff5b4 84 }
osaka 6:1f5e6efff5b4 85
osaka 6:1f5e6efff5b4 86 void Autopilot::keep_alt()
osaka 6:1f5e6efff5b4 87 {
osaka 6:1f5e6efff5b4 88 //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
cocorlow 15:28f480b41553 89 float v2 = vel_ned(0)*vel_ned(0) + vel_ned(1)*vel_ned(1) + vel_ned(2)*vel_ned(2);
osaka 6:1f5e6efff5b4 90 float E = alt + v2 / (2*G);
osaka 13:75636841e43b 91 float alt_sp;
osaka 13:75636841e43b 92 if (alt_obj - alt > 5.0f)
osaka 13:75636841e43b 93 alt_sp = alt + 5.0f;
osaka 13:75636841e43b 94 else if (alt_obj - alt < -5.0f)
osaka 13:75636841e43b 95 alt_sp = alt - 5.0f;
osaka 13:75636841e43b 96 else
osaka 13:75636841e43b 97 alt_sp = alt_obj;
osaka 13:75636841e43b 98 float Esp = alt_sp + vel_obj*vel_obj / (2*G);
NaotoMorita 9:6c3e1d574f86 99 float dT_cruise = 0.0f;
osaka 6:1f5e6efff5b4 100 dT_obj = dT_cruise;
NaotoMorita 11:51a0fedd7745 101 dT_obj += p_control(Esp - E, 0.1f);
osaka 6:1f5e6efff5b4 102
osaka 6:1f5e6efff5b4 103 float B = alt - v2 / (2*G);
osaka 13:75636841e43b 104 float Bsp = alt_sp - vel_obj*vel_obj / (2*G);
NaotoMorita 11:51a0fedd7745 105 pitch_obj = p_control(Bsp - B, 0.05f);
osaka 6:1f5e6efff5b4 106 }
osaka 6:1f5e6efff5b4 107
osaka 14:7b6c3f8b85fb 108 void Autopilot::climb()
osaka 14:7b6c3f8b85fb 109 {
osaka 14:7b6c3f8b85fb 110 //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
cocorlow 15:28f480b41553 111 float Edot = path_ang + vdot.norm() / G;
osaka 14:7b6c3f8b85fb 112 float Edot_sp = path_obj + vdot_obj / G;
osaka 14:7b6c3f8b85fb 113 float dT_cruise = 0.0f;
osaka 14:7b6c3f8b85fb 114 dT_obj = dT_cruise;
osaka 14:7b6c3f8b85fb 115 dT_obj += p_control(Edot_sp - Edot, 10.0f);
osaka 14:7b6c3f8b85fb 116
cocorlow 15:28f480b41553 117 float Bdot = path_ang - vdot.norm() / G;
osaka 14:7b6c3f8b85fb 118 float Bdot_sp = path_obj - vdot_obj / G;
osaka 14:7b6c3f8b85fb 119 pitch_obj = p_control(Bdot_sp - Bdot, 10.0f);
osaka 14:7b6c3f8b85fb 120 }
osaka 14:7b6c3f8b85fb 121
osaka 7:988905aed916 122 void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj)
osaka 0:e9cdfc6579a7 123 {
osaka 0:e9cdfc6579a7 124 limit_obj();
osaka 2:049e057f4625 125 r_obj = this->roll_obj;
osaka 2:049e057f4625 126 p_obj = this->pitch_obj;
osaka 6:1f5e6efff5b4 127 t_obj = this->dT_obj;
osaka 0:e9cdfc6579a7 128 }
osaka 0:e9cdfc6579a7 129
osaka 0:e9cdfc6579a7 130 void Autopilot::limit_obj()
osaka 0:e9cdfc6579a7 131 {
osaka 0:e9cdfc6579a7 132 //roll目標値を±30[deg]に制限
osaka 0:e9cdfc6579a7 133 if (roll_obj > deg2rad(30.0f))
osaka 1:73704460a8b4 134 roll_obj = deg2rad(30.0f);
osaka 0:e9cdfc6579a7 135 else if (roll_obj < deg2rad(-30.0f))
osaka 0:e9cdfc6579a7 136 roll_obj = deg2rad(-30.0f);
osaka 0:e9cdfc6579a7 137
osaka 0:e9cdfc6579a7 138 //pitch目標値を±10[deg]に制限
osaka 0:e9cdfc6579a7 139 if (pitch_obj > deg2rad(10.0f))
osaka 0:e9cdfc6579a7 140 pitch_obj = deg2rad(10.0f);
osaka 0:e9cdfc6579a7 141 else if (pitch_obj < deg2rad(-10.0f))
osaka 0:e9cdfc6579a7 142 pitch_obj = deg2rad(-10.0f);
osaka 6:1f5e6efff5b4 143
osaka 6:1f5e6efff5b4 144 //dT目標値を0~1に制限
osaka 6:1f5e6efff5b4 145 if (dT_obj > 1.0f)
osaka 6:1f5e6efff5b4 146 dT_obj = 1.0f;
NaotoMorita 9:6c3e1d574f86 147 else if (dT_obj < -1.0f)
NaotoMorita 9:6c3e1d574f86 148 dT_obj = -1.0f;
osaka 0:e9cdfc6579a7 149 }
osaka 0:e9cdfc6579a7 150
osaka 6:1f5e6efff5b4 151 float Autopilot::p_control(float diff, float kp)
osaka 0:e9cdfc6579a7 152 {
osaka 6:1f5e6efff5b4 153 return kp * diff;
osaka 0:e9cdfc6579a7 154 }
osaka 0:e9cdfc6579a7 155
osaka 6:1f5e6efff5b4 156 float Autopilot::angdiff_pi(float rad)
osaka 0:e9cdfc6579a7 157 {
cocorlow 15:28f480b41553 158 if (rad > M_PI_F)
cocorlow 15:28f480b41553 159 return rad - 2*M_PI_F;
cocorlow 15:28f480b41553 160 else if (rad < -M_PI_F)
cocorlow 15:28f480b41553 161 return rad + 2*M_PI_F;
osaka 0:e9cdfc6579a7 162 else
osaka 0:e9cdfc6579a7 163 return rad;
osaka 0:e9cdfc6579a7 164 }
osaka 0:e9cdfc6579a7 165
NaotoMorita 11:51a0fedd7745 166 float Autopilot::atan_angdiff(float a1,float a2)
NaotoMorita 11:51a0fedd7745 167 {
cocorlow 16:36d034dec6d6 168 return std::atan2(std::sin(a1-a2),std::cos(a1-a2));
NaotoMorita 11:51a0fedd7745 169 }
NaotoMorita 11:51a0fedd7745 170
osaka 0:e9cdfc6579a7 171 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 172 {
cocorlow 15:28f480b41553 173 return deg * M_PI_F / 180.0f;
osaka 14:7b6c3f8b85fb 174 }
osaka 14:7b6c3f8b85fb 175
osaka 14:7b6c3f8b85fb 176 float Autopilot::calc_path()
osaka 14:7b6c3f8b85fb 177 {
osaka 14:7b6c3f8b85fb 178 float alt_diff = alt - alt_before;
cocorlow 15:28f480b41553 179 float xy_diff = sqrt((pos_ned(0) - pos_before(0))*(pos_ned(0) - pos_before(0)) + (pos_ned(1) - pos_before(1))*(pos_ned(1) - pos_before(1)));
cocorlow 16:36d034dec6d6 180 path_ang = std::atan2(alt_diff, xy_diff);
osaka 0:e9cdfc6579a7 181 }