Diff: Autopilot.cpp

Revision:

6:1f5e6efff5b4

Parent:

4:1117ad251fef

Child:

7:988905aed916

diff -r 1117ad251fef -r 1f5e6efff5b4 Autopilot.cpp
--- a/Autopilot.cpp	Mon Nov 15 13:41:51 2021 +0000
+++ b/Autopilot.cpp	Fri Nov 19 05:18:37 2021 +0000
@@ -4,58 +4,6 @@
 {
 }
 
-void Autopilot::update_val(const Vector3 rpy, const float altitude, const 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 * (angdif_pi(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;
@@ -69,12 +17,81 @@
     this->turn_r = r;
 }
 
-void Autopilot::return_val(float &r_obj, float &p_obj, float &a_obj)
+void Autopilot:set_alt(float alt, float vel)
+{
+    this->alt_obj = alt;
+    this->vel_obj = vel;
+}
+
+void Autopilot::update_val(const Vector3 rpy, const float altitude, const Vector3 pos, const Vector3 vel)
+{
+    this->roll = rpy.x;
+    this->pitch = rpy.y;
+    this->yaw = rpy.z;
+    this->alt = altitude;
+    this->pos_ned = pos;
+    this->vel_ned = vel;
+}
+
+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_diff = angdiff_pi(yaw_obj - yaw);
+    roll_obj = p_control(yaw_diff, 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_diff = angdiff_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 * (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);   //角度のずれが大きいときは角度合わせを優先
+    }
+}
+
+void Autopilot::keep_alt()
+{
+    //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
+    float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z;
+    float E = alt + v2 / (2*G);
+    float Esp = alt_obj + vel_obj*vel_obj / (2*G);
+    float dT_cruise = 0.4f;
+    dT_obj = dT_cruise;
+    dT_obj += p_control(Esp - E, 0.05f);
+    
+    float B = alt - v2 / (2*G);
+    float Bsp = alt_obj - vel_obj*vel_obj / (2*G);
+    pitch_obj = p_control(Bsp - B, 0.02f);
+}
+
+void Autopilot::return_val(float &r_obj, float &p_obj, float t_obj)
 {
     limit_obj();
     r_obj = this->roll_obj;
     p_obj = this->pitch_obj;
-    a_obj = this->alt_obj;
+    t_obj = this->dT_obj;
 }
 
 void Autopilot::limit_obj()
@@ -90,14 +107,20 @@
         pitch_obj = deg2rad(10.0f);
     else if (pitch_obj < deg2rad(-10.0f))
         pitch_obj = deg2rad(-10.0f);
+        
+    //dT目標値を0~1に制限
+    if (dT_obj > 1.0f)
+        dT_obj = 1.0f;
+    else if (dT_obj < 0.0f)
+        dT_obj = 0.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;