Diff: Autopilot.cpp

Revision:

14:7b6c3f8b85fb

Parent:

13:75636841e43b

Child:

15:28f480b41553

--- a/Autopilot.cpp	Wed Dec 01 09:23:26 2021 +0000
+++ b/Autopilot.cpp	Wed Dec 01 11:51:21 2021 +0000
@@ -23,18 +23,28 @@
     this->vel_obj = vel;
 }
 
-void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel)
+void Autopilot::set_climb(float path_ang, float vdot)
+{
+    this->path_obj = path_ang;
+    this->vdot_obj = vdot;
+}
+
+void Autopilot::update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot)
 {
     this->roll = rpy.x;
     this->pitch = rpy.y;
     this->yaw = rpy.z;
+    this->alt_before = this->alt;
     this->alt = altitude;
+    this->pos_before = this->pos_ned;
     this->pos_ned.x = pos(1, 1);
     this->pos_ned.y = pos(2, 1);
     this->pos_ned.z = pos(3, 1);
+    this->vel_before = this->vel_ned;
     this->vel_ned.x = vel(1, 1);
     this->vel_ned.y = vel(2, 1);
     this->vel_ned.z = vel(3, 1);
+    this->vdot = v_dot;
 }
 
 void Autopilot::level()
@@ -95,6 +105,20 @@
     pitch_obj = p_control(Bsp - B, 0.05f);
 }
 
+void Autopilot::climb()
+{
+    //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
+    float Edot = path_ang + vdot.Norm() / G;
+    float Edot_sp = path_obj + vdot_obj / G;
+    float dT_cruise = 0.0f;
+    dT_obj = dT_cruise;
+    dT_obj += p_control(Edot_sp - Edot, 10.0f);
+    
+    float Bdot = path_ang - vdot.Norm() / G;
+    float Bdot_sp = path_obj - vdot_obj / G;
+    pitch_obj = p_control(Bdot_sp - Bdot, 10.0f);
+}
+
 void Autopilot::return_val(float &r_obj, float &p_obj, float &t_obj)
 {
     limit_obj();
@@ -147,4 +171,11 @@
 float Autopilot::deg2rad(float deg)
 {
     return deg * M_PI / 180.0f;
+}
+
+float Autopilot::calc_path()
+{
+    float alt_diff = alt - alt_before;
+    float xy_diff = sqrt((pos_ned.x - pos_before.x)*(pos_ned.x - pos_before.x) + (pos_ned.y - pos_before.y)*(pos_ned.y - pos_before.y));
+    path_ang = atan2f(alt_diff, xy_diff);
 }
\ No newline at end of file