Diff: Autopilot.cpp

Revision:

5:9a42b7d85a6b

Parent:

3:46d4a32011fc

--- 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;