Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
Diff: Autopilot.cpp
- Revision:
- 5:9a42b7d85a6b
- Parent:
- 3:46d4a32011fc
diff -r 46d4a32011fc -r 9a42b7d85a6b Autopilot.cpp --- a/Autopilot.cpp Fri Nov 12 12:55:25 2021 +0000 +++ b/Autopilot.cpp Mon Nov 15 03:50:10 2021 +0000 @@ -23,8 +23,8 @@ 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); + float yaw_diff = angdiff_pi(yaw_obj - yaw); + roll_obj = p_control(yaw_diff, 0.3f); } void Autopilot::turn() @@ -32,7 +32,7 @@ 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 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) @@ -43,8 +43,8 @@ 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.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); //角度のずれが大きいときは角度合わせを優先 @@ -92,12 +92,12 @@ pitch_obj = deg2rad(-10.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;