Diff: angle_estimator.cpp

Revision:

0:acf871f26563

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle_estimator.cpp	Fri Mar 08 13:34:59 2019 +0000
@@ -0,0 +1,42 @@
+#include "angle_estimator.h"
+#include "mbed.h"
+
+#define PI 3.1415927
+
+
+using namespace std;
+
+angle_estimator::angle_estimator(float Ts) : ax(PA_0), ay(PA_1), gz(PA_4){
+    u2ax.setup(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
+    u2ay.setup(0.29890,0.70330f,-9.81f,9.81f);      // use normalized input
+    u2gz.setup(-4.6517f * 3.3f,0.45495f);           // use normalized input
+                // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
+    float tau = 1.0f;
+    f_ax.setup(tau,Ts,1.0);
+    f_ay.setup(tau,Ts,1.0);
+    f_gz.setup(tau,Ts,tau);
+
+    }
+
+
+angle_estimator::~angle_estimator() {}
+
+
+
+// ************ calculate angle *****************
+void angle_estimator::calc_angle_phi1(int do_reset){
+    gyro = u2gz(gz.read());
+    if(do_reset == 1){
+        f_ax.reset(u2ax(ax.read()));
+        f_ay.reset(u2ax(ay.read()));
+        f_gz.reset(gyro);
+        }
+    phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
+    }
+
+float angle_estimator::get_phi1(void){
+return phi1;
+}
+float angle_estimator::get_gyro(void){
+return gyro;
+}
\ No newline at end of file