Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Diff: Autopilot.cpp
- Revision:
- 6:1f5e6efff5b4
- Parent:
- 4:1117ad251fef
- Child:
- 7:988905aed916
diff -r 1117ad251fef -r 1f5e6efff5b4 Autopilot.cpp --- a/Autopilot.cpp Mon Nov 15 13:41:51 2021 +0000 +++ b/Autopilot.cpp Fri Nov 19 05:18:37 2021 +0000 @@ -4,58 +4,6 @@ { } -void Autopilot::update_val(const Vector3 rpy, const float altitude, const Vector3 pos) -{ - this->roll = rpy.x; - this->pitch = rpy.y; - this->yaw = rpy.z; - this->alt = altitude; - this->pos_ned = pos; -} - -void Autopilot::level() -{ - roll_obj = 0.0f; -} - -void Autopilot::guide() -{ - float x_dist = destination.x - pos_ned.x; - float y_dist = destination.y - pos_ned.y; - yaw_obj = std::atan2(y_dist, x_dist); //rad - float yaw_dif = angdif_pi(yaw_obj - yaw); - roll_obj = p_control(yaw_dif, 0.3f); -} - -void Autopilot::turn() -{ - float x_dist = turn_center.x - pos_ned.x; - float y_dist = turn_center.y - pos_ned.y; - float yaw_center = std::atan2(y_dist, x_dist); - float yaw_dif = angdif_pi(yaw_center - yaw); //旋回中心までの方位角, rad - float dist = std::sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離 - //旋回円から遠い場合は旋回中心まで誘導 - if (dist > 2*turn_r) - { - set_dest(turn_center.x, turn_center.y); - guide(); - } - else - { - roll_obj = 0.35f; //基準 - roll_obj -= 0.6f * (angdif_pi(0.5f*M_PI - yaw_dif)); //旋回中心方向に対する角度を90[deg]にする - if (std::abs(0.5f*M_PI - yaw_dif) < deg2rad(5)) //距離を保つ - roll_obj -= 0.035f * (turn_r - dist); - else - roll_obj -= 0.01f * (turn_r - dist); //角度のずれが大きいときは角度合わせを優先 - } -} - -void Autopilot::keep_alt() -{ - -} - void Autopilot::set_dest(float x, float y) { this->destination.x = x; @@ -69,12 +17,81 @@ this->turn_r = r; } -void Autopilot::return_val(float &r_obj, float &p_obj, float &a_obj) +void Autopilot:set_alt(float alt, float vel) +{ + this->alt_obj = alt; + this->vel_obj = vel; +} + +void Autopilot::update_val(const Vector3 rpy, const float altitude, const Vector3 pos, const Vector3 vel) +{ + this->roll = rpy.x; + this->pitch = rpy.y; + this->yaw = rpy.z; + this->alt = altitude; + this->pos_ned = pos; + this->vel_ned = vel; +} + +void Autopilot::level() +{ + roll_obj = 0.0f; +} + +void Autopilot::guide() +{ + float x_dist = destination.x - pos_ned.x; + float y_dist = destination.y - pos_ned.y; + yaw_obj = std::atan2(y_dist, x_dist); //rad + float yaw_diff = angdiff_pi(yaw_obj - yaw); + roll_obj = p_control(yaw_diff, 0.3f); +} + +void Autopilot::turn() +{ + float x_dist = turn_center.x - pos_ned.x; + float y_dist = turn_center.y - pos_ned.y; + float yaw_center = std::atan2(y_dist, x_dist); + float yaw_diff = angdiff_pi(yaw_center - yaw); //旋回中心までの方位角, rad + float dist = std::sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離 + //旋回円から遠い場合は旋回中心まで誘導 + if (dist > 2*turn_r) + { + set_dest(turn_center.x, turn_center.y); + guide(); + } + else + { + roll_obj = 0.35f; //基準 + roll_obj -= 0.6f * (angdiff_pi(0.5f*M_PI - yaw_diff)); //旋回中心方向に対する角度を90[deg]にする + if (std::abs(0.5f*M_PI - yaw_diff) < deg2rad(5)) //距離を保つ + roll_obj -= 0.035f * (turn_r - dist); + else + roll_obj -= 0.01f * (turn_r - dist); //角度のずれが大きいときは角度合わせを優先 + } +} + +void Autopilot::keep_alt() +{ + //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html), p制御のみ + float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z; + float E = alt + v2 / (2*G); + float Esp = alt_obj + vel_obj*vel_obj / (2*G); + float dT_cruise = 0.4f; + dT_obj = dT_cruise; + dT_obj += p_control(Esp - E, 0.05f); + + float B = alt - v2 / (2*G); + float Bsp = alt_obj - vel_obj*vel_obj / (2*G); + pitch_obj = p_control(Bsp - B, 0.02f); +} + +void Autopilot::return_val(float &r_obj, float &p_obj, float t_obj) { limit_obj(); r_obj = this->roll_obj; p_obj = this->pitch_obj; - a_obj = this->alt_obj; + t_obj = this->dT_obj; } void Autopilot::limit_obj() @@ -90,14 +107,20 @@ pitch_obj = deg2rad(10.0f); else if (pitch_obj < deg2rad(-10.0f)) pitch_obj = deg2rad(-10.0f); + + //dT目標値を0~1に制限 + if (dT_obj > 1.0f) + dT_obj = 1.0f; + else if (dT_obj < 0.0f) + dT_obj = 0.0f; } -float Autopilot::p_control(float dif, float kp) +float Autopilot::p_control(float diff, float kp) { - return kp * dif; + return kp * diff; } -float Autopilot::angdif_pi(float rad) +float Autopilot::angdiff_pi(float rad) { if (rad > M_PI) return rad - 2*M_PI;