Diff: Autopilot.cpp

Revision:

2:049e057f4625

Parent:

1:73704460a8b4

Child:

3:46d4a32011fc

--- a/Autopilot.cpp	Fri Nov 12 09:03:10 2021 +0000
+++ b/Autopilot.cpp	Fri Nov 12 12:16:38 2021 +0000
@@ -34,10 +34,10 @@
     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
@@ -69,14 +69,12 @@
     this->turn_r = r;
 }
 
-std::vector<float> Autopilot::return_val()
+void Autopilot::return_val(float &r_obj, float &p_obj, float &a_obj)
 {
     limit_obj();
-    std::vector<float> obj;
-    obj[0] = roll_obj;
-    obj[1] = pitch_obj;
-    obj[2] = alt_obj;
-    return obj;
+    r_obj = this->roll_obj;
+    p_obj = this->pitch_obj;
+    a_obj = this->alt_obj;
 }
 
 void Autopilot::limit_obj()