HAPSRG / Autopilot_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen

Autopilot.cpp@14:7b6c3f8b85fb, 2021-12-01 (annotated)

Committer:
osaka
Date:
Wed Dec 01 11:51:21 2021 +0000
Revision:
14:7b6c3f8b85fb
Parent:
13:75636841e43b
Child:
15:28f480b41553
with path angle control

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 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
osaka 14:7b6c3f8b85fb 32 void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot)
osaka 6:1f5e6efff5b4 33 {
osaka 6:1f5e6efff5b4 34 this->roll = rpy.x;
osaka 6:1f5e6efff5b4 35 this->pitch = rpy.y;
osaka 6:1f5e6efff5b4 36 this->yaw = rpy.z;
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;
osaka 12:59e547742cd8 40 this->pos_ned.x = pos(1, 1);
osaka 12:59e547742cd8 41 this->pos_ned.y = pos(2, 1);
osaka 12:59e547742cd8 42 this->pos_ned.z = pos(3, 1);
osaka 14:7b6c3f8b85fb 43 this->vel_before = this->vel_ned;
osaka 12:59e547742cd8 44 this->vel_ned.x = vel(1, 1);
osaka 12:59e547742cd8 45 this->vel_ned.y = vel(2, 1);
osaka 12:59e547742cd8 46 this->vel_ned.z = vel(3, 1);
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 {
osaka 6:1f5e6efff5b4 57 float x_dist = destination.x - pos_ned.x;
osaka 6:1f5e6efff5b4 58 float y_dist = destination.y - pos_ned.y;
NaotoMorita 11:51a0fedd7745 59 yaw_obj = atan2f(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 {
osaka 6:1f5e6efff5b4 66 float x_dist = turn_center.x - pos_ned.x;
osaka 6:1f5e6efff5b4 67 float y_dist = turn_center.y - pos_ned.y;
NaotoMorita 11:51a0fedd7745 68 float yaw_center = atan2f(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 {
osaka 6:1f5e6efff5b4 74 set_dest(turn_center.x, turn_center.y);
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; //基準
NaotoMorita 11:51a0fedd7745 80 float yaw_obj = 0.5f*M_PI+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制御のみ
osaka 6:1f5e6efff5b4 89 float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z;
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制御のみ
osaka 14:7b6c3f8b85fb 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
osaka 14:7b6c3f8b85fb 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 {
osaka 1:73704460a8b4 158 if (rad > M_PI)
osaka 1:73704460a8b4 159 return rad - 2*M_PI;
osaka 1:73704460a8b4 160 else if (rad < -M_PI)
osaka 1:73704460a8b4 161 return rad + 2*M_PI;
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 {
NaotoMorita 11:51a0fedd7745 168 return atan2f(sinf(a1-a2),cosf(a1-a2));
NaotoMorita 11:51a0fedd7745 169 }
NaotoMorita 11:51a0fedd7745 170
osaka 0:e9cdfc6579a7 171 float Autopilot::deg2rad(float deg)
osaka 0:e9cdfc6579a7 172 {
osaka 1:73704460a8b4 173 return deg * M_PI / 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;
osaka 14:7b6c3f8b85fb 179 float xy_diff = sqrt((pos_ned.x - pos_before.x)*(pos_ned.x - pos_before.x) + (pos_ned.y - pos_before.y)*(pos_ned.y - pos_before.y));
osaka 14:7b6c3f8b85fb 180 path_ang = atan2f(alt_diff, xy_diff);
osaka 0:e9cdfc6579a7 181 }