Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.cpp
- Committer:
- NaotoMorita
- Date:
- 2022-06-26
- Revision:
- 18:5e2e03a30e1c
- Parent:
- 16:36d034dec6d6
File content as of revision 18:5e2e03a30e1c:
#include "Autopilot.hpp" Autopilot::Autopilot() { } void Autopilot::set_dest(float x, float y) { this->destination(0) = x; this->destination(1) = y; } void Autopilot::set_turn(float x, float y, float r) { this->turn_center(0) = x; this->turn_center(1) = y; this->turn_r = r; } void Autopilot::set_alt(float alt, float vel) { this->alt_obj = alt; this->vel_obj = vel; } //void Autopilot::set_climb(float path_ang, float vdot) //{ // this->path_obj = path_ang; // this->vdot_obj = vdot; //} void Autopilot::update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel) { this->roll = rpy(0); this->pitch = rpy(1); this->yaw = rpy(2); this->alt_before = this->alt; this->alt = altitude; this->pos_before = this->pos_ned; this->pos_ned(0) = pos(0); this->pos_ned(1) = pos(1); this->pos_ned(2) = pos(2); this->vel_before = this->vel_ned; this->vel_ned(0) = vel(0); this->vel_ned(1) = vel(1); this->vel_ned(2) = vel(2); //this->vdot = v_dot; } void Autopilot::level() { roll_obj = 0.0f; } void Autopilot::guide() { float x_dist = destination(0) - pos_ned(0); float y_dist = destination(1) - pos_ned(1); yaw_obj = std::atan2(y_dist, x_dist); //rad float yaw_diff = atan_angdiff(yaw_obj,yaw); roll_obj = p_control(yaw_diff, 0.3f); } void Autopilot::turn() { float x_dist = turn_center(0) - pos_ned(0); float y_dist = turn_center(1) - pos_ned(1); float yaw_center = std::atan2(y_dist, x_dist); float yaw_diff = atan_angdiff(yaw_center,yaw); //旋回中心までの方位角, rad float dist = sqrt(x_dist*x_dist + y_dist*y_dist); //旋回中心までの距離 //旋回円から遠い場合は旋回中心まで誘導 if (dist > 2*turn_r) { set_dest(turn_center(0), turn_center(1)); guide(); } else { roll_obj = 0.35f; //基準 float yaw_obj = 0.5f*M_PI_F+0.05f*(turn_r - dist); float coasediff = (atan_angdiff(yaw_obj,yaw_diff)); roll_obj -= 0.4f * coasediff; //旋回中心方向に対する角度を90[deg]にする } } void Autopilot::keep_alt() { //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html), p制御のみ float v2 = vel_ned(0)*vel_ned(0) + vel_ned(1)*vel_ned(1) + vel_ned(2)*vel_ned(2); float E = alt + v2 / (2*G); float alt_sp; if (alt_obj - alt > 5.0f) alt_sp = alt + 5.0f; else if (alt_obj - alt < -5.0f) alt_sp = alt - 5.0f; else alt_sp = alt_obj; float Esp = alt_sp + vel_obj*vel_obj / (2*G); float dT_cruise = 0.0f; dT_obj = dT_cruise; dT_obj += p_control(Esp - E, 0.1f); float B = alt - v2 / (2*G); float Bsp = alt_sp - vel_obj*vel_obj / (2*G); pitch_obj = p_control(Bsp - B, 0.05f); } //void Autopilot::climb() //{ // //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html), p制御のみ // float Edot = path_ang + vdot.norm() / G; // float Edot_sp = path_obj + vdot_obj / G; // float dT_cruise = 0.0f; // dT_obj = dT_cruise; // dT_obj += p_control(Edot_sp - Edot, 10.0f); // float Bdot = path_ang - vdot.norm() / G; // float Bdot_sp = path_obj - vdot_obj / G; // pitch_obj = p_control(Bdot_sp - Bdot, 10.0f); //} 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; t_obj = this->dT_obj; } void Autopilot::limit_obj() { //roll目標値を±30[deg]に制限 if (roll_obj > deg2rad(30.0f)) roll_obj = deg2rad(30.0f); else if (roll_obj < deg2rad(-30.0f)) roll_obj = deg2rad(-30.0f); //pitch目標値を±10[deg]に制限 if (pitch_obj > deg2rad(10.0f)) 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 < -1.0f) dT_obj = -1.0f; } float Autopilot::p_control(float diff, float kp) { return kp * diff; } float Autopilot::angdiff_pi(float rad) { if (rad > M_PI_F) return rad - 2*M_PI_F; else if (rad < -M_PI_F) return rad + 2*M_PI_F; else return rad; } float Autopilot::atan_angdiff(float a1,float a2) { return std::atan2(std::sin(a1-a2),std::cos(a1-a2)); } float Autopilot::deg2rad(float deg) { return deg * M_PI_F / 180.0f; } //float Autopilot::calc_path() //{ // float alt_diff = alt - alt_before; // 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))); // path_ang = std::atan2(alt_diff, xy_diff); //}