Diff: Autopilot.cpp

Revision:

1:73704460a8b4

Parent:

0:e9cdfc6579a7

Child:

2:049e057f4625

diff -r e9cdfc6579a7 -r 73704460a8b4 Autopilot.cpp
--- a/Autopilot.cpp	Fri Nov 12 06:50:45 2021 +0000
+++ b/Autopilot.cpp	Fri Nov 12 09:03:10 2021 +0000
@@ -4,7 +4,7 @@
 {
 }
 
-void Autopilot::update_val(Vector3 rpy, float altitude, vector3 pos)
+void Autopilot::update_val(Vector3 rpy, float altitude, Vector3 pos)
 {
     this->roll = rpy.x;
     this->pitch = rpy.y;
@@ -31,7 +31,7 @@
 {
     float x_dist = turn_center.x - pos_ned.x;
     float y_dist = turn_center.y - pos_ned.y;
-    yaw_center = std::atan2(y_dist, x_dist);
+    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);    //旋回中心までの距離
     
@@ -43,8 +43,8 @@
     else
     {
         roll_obj = 0.35f;   //基準
-        roll_obj -= 0.6f * (0.5f*pi - yaw_dif);     //旋回中心方向に対する角度を90[deg]にする
-        if (std::abs(0.5f*pi - yaw_dif) < deg2rad(5))   //距離を保つ
+        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);   //角度のずれが大きいときは角度合わせを優先
@@ -56,28 +56,34 @@
     
 }
 
-void Autopilot::set_dest(Vector3 dest)
+void Autopilot::set_dest(float x, float y)
 {
-    this->destination = dest;
+    this->destination.x = x;
+    this->destination.y = y;
 }
 
-void Autopilot::set_turn(Vector3 center, float r)
+void Autopilot::set_turn(float x, float y, float r)
 {
-    this->turn_center = center;
+    this->turn_center.x = x;
+    this->turn_center.y = y;
     this->turn_r = r;
 }
 
 std::vector<float> Autopilot::return_val()
 {
     limit_obj();
-    return {roll_obj, pitch_obj, alt_obj};
+    std::vector<float> obj;
+    obj[0] = roll_obj;
+    obj[1] = pitch_obj;
+    obj[2] = alt_obj;
+    return obj;
 }
 
 void Autopilot::limit_obj()
 {
     //roll目標値を±30[deg]に制限
     if (roll_obj > deg2rad(30.0f))
-        roll_objj = deg2rad(30.0f);
+        roll_obj = deg2rad(30.0f);
     else if (roll_obj < deg2rad(-30.0f))
         roll_obj = deg2rad(-30.0f);
 
@@ -95,15 +101,15 @@
 
 float Autopilot::angdif_pi(float rad)
 {
-    if (rad > pi)   
-        return rad - 2*pi;
-    else if (rad < -pi)     
-        return rad + 2*pi;
+    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 * pi / 180.0f;
+    return deg * M_PI / 180.0f;
 }
\ No newline at end of file