Diff: Autopilot.cpp

Revision:

18:5e2e03a30e1c

Parent:

16:36d034dec6d6

--- a/Autopilot.cpp	Fri Dec 10 10:41:44 2021 +0000
+++ b/Autopilot.cpp	Sun Jun 26 12:04:04 2022 +0000
@@ -23,13 +23,13 @@
     this->vel_obj = vel;
 }
 
-void Autopilot::set_climb(float path_ang, float vdot)
-{
-    this->path_obj = path_ang;
-    this->vdot_obj = vdot;
-}
+//void Autopilot::set_climb(float path_ang, float vdot)
+//{
+//    this->path_obj = path_ang;
+//    this->vdot_obj = vdot;
+//}
 
-void Autopilot::update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel, const Vector3f v_dot)
+void Autopilot::update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel)
 {
     this->roll = rpy(0);
     this->pitch = rpy(1);
@@ -44,7 +44,7 @@
     this->vel_ned(0) = vel(0);
     this->vel_ned(1) = vel(1);
     this->vel_ned(2) = vel(2);
-    this->vdot = v_dot;
+    //this->vdot = v_dot;
 }
 
 void Autopilot::level()
@@ -105,19 +105,19 @@
     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);
+//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);
-}
+ //   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)
 {
@@ -173,9 +173,9 @@
     return deg * M_PI_F / 180.0f;
 }
 
-float Autopilot::calc_path()
-{
-    float alt_diff = alt - alt_before;
-    float xy_diff = sqrt((pos_ned(0) - pos_before(0))*(pos_ned(0) - pos_before(0)) + (pos_ned(1) - pos_before(1))*(pos_ned(1) - pos_before(1)));
-    path_ang = std::atan2(alt_diff, xy_diff);
-}
\ No newline at end of file
+//float Autopilot::calc_path()
+//{
+//    float alt_diff = alt - alt_before;
+//    float xy_diff = sqrt((pos_ned(0) - pos_before(0))*(pos_ned(0) - pos_before(0)) + (pos_ned(1) - pos_before(1))*(pos_ned(1) - pos_before(1)));
+//    path_ang = std::atan2(alt_diff, xy_diff);
+//}
\ No newline at end of file