Diff: autopilot.cpp

Revision:

140:53dbdb207542

Parent:

139:b378528c05f2

Child:

143:53808e4e684c

--- a/autopilot.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/autopilot.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,52 +1,53 @@
-//#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()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f 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()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f 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()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f 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()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.level();
+    autopilot.climb();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+
+Vector3f calc_vdot()
+{
+    Vector3f m_vdot = eskf.calcDynAcc(acc);
+    Vector3f vdot;
+    vdot = m_vdot;
+    return vdot;
+}
\ No newline at end of file