Diff: autopilot.cpp

Revision:

137:240588882ae2

Parent:

131:45542fcc886f

Child:

139:b378528c05f2

--- a/autopilot.cpp	Wed Dec 01 09:24:24 2021 +0000
+++ b/autopilot.cpp	Wed Dec 01 11:51:49 2021 +0000
@@ -2,9 +2,10 @@
  
 void level_flight()
 {
+    Vector3 vdot = calc_vdot();
     Matrix pihat = eskf.getPihat();
     Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat);
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
     autopilot.level();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -12,9 +13,10 @@
  
 void point_guide()
 {
+    Vector3 vdot = calc_vdot();
     Matrix pihat = eskf.getPihat();
     Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat);
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
     autopilot.guide();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
@@ -22,10 +24,29 @@
  
 void turning()
 {
+    Vector3 vdot = calc_vdot();
     Matrix pihat = eskf.getPihat();
     Matrix vihat = eskf.getVihat();
-    autopilot.update_val(rpy, -palt, pihat, vihat);
+    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