Diff: Autopilot.hpp

Revision:

15:28f480b41553

Parent:

14:7b6c3f8b85fb

Child:

17:598522e1bf4f

--- a/Autopilot.hpp	Wed Dec 01 11:51:21 2021 +0000
+++ b/Autopilot.hpp	Mon Dec 06 08:23:54 2021 +0000
@@ -3,11 +3,18 @@
 
 #include "mbed.h"
 #include "Vector3.hpp"
-#include "Matrix.h"
+#include <Eigen/Core.h>
 #include <cmath>
 
-#define M_PI 3.141592f
+using namespace Eigen;
+
+#ifndef M_PI_F
+#define M_PI_F 3.141592f
+#endif
+
+#ifndef G
 #define G 9.8f
+#endif
 
 class Autopilot
 {
@@ -18,14 +25,14 @@
     float yaw;
     float alt;
     float alt_before;
-    Vector3 pos_ned;
-    Vector3 pos_before;
-    Vector3 vel_ned;    //対地速度
-    Vector3 vel_before;
+    Vector3f pos_ned;
+    Vector3f pos_before;
+    Vector3f vel_ned;    //対地速度
+    Vector3f vel_before;
     
     //算出値
     float path_ang;    //経路角
-    Vector3 vdot;
+    Vector3f vdot;
     
     //目標値
     float roll_obj;
@@ -37,8 +44,8 @@
     float path_obj;
     float vdot_obj; 
     
-    Vector3 destination;    //誘導地点NED座標
-    Vector3 turn_center;    //旋回中心NED座標
+    Vector3f destination;    //誘導地点NED座標
+    Vector3f turn_center;    //旋回中心NED座標
     float turn_r;   //旋回半径
     
     void limit_obj();
@@ -56,7 +63,7 @@
     void set_alt(float alt, float vel);     //高度，速度の設定
     void set_climb(float path_angle, float vdot);     //経路角,加速度の設定
     
-    void update_val(const Vector3 rpy, const float altitude, const Matrix pos, const Matrix vel, const Vector3 v_dot);   //姿勢角等の更新
+    void update_val(const Vector3f rpy, const float altitude, const Vector3f pos, const Vector3f vel, const Vector3f v_dot);   //姿勢角等の更新
     
     void level();    //水平飛行
     void guide();    //定点誘導