Diff: autopilot.cpp

Revision:

139:b378528c05f2

Parent:

137:240588882ae2

Child:

140:53dbdb207542

--- a/autopilot.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/autopilot.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -1,52 +1,52 @@
-#include "global.hpp"
- 
-void level_flight()
-{
-    Vector3 vdot = calc_vdot();
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-    autopilot.level();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
- 
-void point_guide()
-{
-    Vector3 vdot = calc_vdot();
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-    autopilot.guide();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
-}
- 
-void turning()
-{
-    Vector3 vdot = calc_vdot();
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-    autopilot.turn();
-    autopilot.keep_alt();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
-
-void climb()
-{
-    Vector3 vdot = calc_vdot();
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-    autopilot.level();
-    autopilot.climb();
-    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-}
-
-Vector3 calc_vdot()
-{
-    Matrix m_vdot = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
-    Vector3 vdot(m_vdot(1, 1), m_vdot(2, 1), m_vdot(3, 1));
-    return vdot;
-}
\ No newline at end of file
+//#include "global.hpp"
+// 
+//void level_flight()
+//{
+//    Vector3 vdot = calc_vdot();
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+//    autopilot.level();
+//    autopilot.keep_alt();
+//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+//}
+// 
+//void point_guide()
+//{
+//    Vector3 vdot = calc_vdot();
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+//    autopilot.guide();
+//    autopilot.keep_alt();
+//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
+//}
+// 
+//void turning()
+//{
+//    Vector3 vdot = calc_vdot();
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+//    autopilot.turn();
+//    autopilot.keep_alt();
+//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+//}
+//
+//void climb()
+//{
+//    Vector3 vdot = calc_vdot();
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+//    autopilot.level();
+//    autopilot.climb();
+//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+//}
+//
+//Vector3 calc_vdot()
+//{
+//    Matrix m_vdot = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
+//    Vector3 vdot(m_vdot(1, 1), m_vdot(2, 1), m_vdot(3, 1));
+//    return vdot;
+//}
\ No newline at end of file