Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Autopilot.cpp
- Committer:
- osaka
- Date:
- 2021-11-12
- Revision:
- 2:049e057f4625
- Parent:
- 1:73704460a8b4
- Child:
- 3:46d4a32011fc
File content as of revision 2:049e057f4625:
#include "Autopilot.hpp" Autopilot::Autopilot() { } void Autopilot::update_val(Vector3 rpy, float altitude, 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 * (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; this->destination.y = y; } void Autopilot::set_turn(float x, float y, float r) { this->turn_center.x = x; this->turn_center.y = y; this->turn_r = r; } void Autopilot::return_val(float &r_obj, float &p_obj, float &a_obj) { limit_obj(); r_obj = this->roll_obj; p_obj = this->pitch_obj; a_obj = this->alt_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); } float Autopilot::p_control(float dif, float kp) { return kp * dif; } float Autopilot::angdif_pi(float rad) { if (rad > M_PI) return rad - 2*M_PI; else if (rad < -M_PI) return rad + 2*M_PI; else return rad; } float Autopilot::deg2rad(float deg) { return deg * M_PI / 180.0f; }