HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

Diff: autopilot.cpp

Revision:
131:45542fcc886f
Parent:
130:3a0ab2affb0f
Child:
137:240588882ae2
--- a/autopilot.cpp	Mon Nov 29 12:33:47 2021 +0000
+++ b/autopilot.cpp	Mon Nov 29 13:13:34 2021 +0000
@@ -2,7 +2,9 @@
  
 void level_flight()
 {
-    autopilot.update_val(rpy, -palt, pi, vi);
+    Matrix pihat = eskf.getPihat();
+    Matrix vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat);
     autopilot.level();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);
@@ -10,7 +12,9 @@
  
 void point_guide()
 {
-    autopilot.update_val(rpy, -palt, pi, vi);
+    Matrix pihat = eskf.getPihat();
+    Matrix vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat);
     autopilot.guide();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
@@ -18,7 +22,9 @@
  
 void turning()
 {
-    autopilot.update_val(rpy, -palt, pi, vi);
+    Matrix pihat = eskf.getPihat();
+    Matrix vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat);
     autopilot.turn();
     autopilot.keep_alt();
     autopilot.return_val(roll_obj, pitch_obj, dT_obj);