altb_pmic / Mbed OS cuboid_balance_ros

....

Dependencies:   Library_Cntrl Library_Misc_cuboid

Fork of cuboid_balance_ros by Ruprecht Altenburger

angle_estimator.cpp@0:acf871f26563, 2019-03-08 (annotated)

Committer:
altb2
Date:
Fri Mar 08 13:34:59 2019 +0000
Revision:
0:acf871f26563
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:acf871f26563 1 #include "angle_estimator.h"
altb2 0:acf871f26563 2 #include "mbed.h"
altb2 0:acf871f26563 3
altb2 0:acf871f26563 4 #define PI 3.1415927
altb2 0:acf871f26563 5
altb2 0:acf871f26563 6
altb2 0:acf871f26563 7 using namespace std;
altb2 0:acf871f26563 8
altb2 0:acf871f26563 9 angle_estimator::angle_estimator(float Ts) : ax(PA_0), ay(PA_1), gz(PA_4){
altb2 0:acf871f26563 10 u2ax.setup(0.29719f,0.69768f,-9.81f,9.81f); // use normalized input
altb2 0:acf871f26563 11 u2ay.setup(0.29890,0.70330f,-9.81f,9.81f); // use normalized input
altb2 0:acf871f26563 12 u2gz.setup(-4.6517f * 3.3f,0.45495f); // use normalized input
altb2 0:acf871f26563 13 // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
altb2 0:acf871f26563 14 float tau = 1.0f;
altb2 0:acf871f26563 15 f_ax.setup(tau,Ts,1.0);
altb2 0:acf871f26563 16 f_ay.setup(tau,Ts,1.0);
altb2 0:acf871f26563 17 f_gz.setup(tau,Ts,tau);
altb2 0:acf871f26563 18
altb2 0:acf871f26563 19 }
altb2 0:acf871f26563 20
altb2 0:acf871f26563 21
altb2 0:acf871f26563 22 angle_estimator::~angle_estimator() {}
altb2 0:acf871f26563 23
altb2 0:acf871f26563 24
altb2 0:acf871f26563 25
altb2 0:acf871f26563 26 // ************ calculate angle *****************
altb2 0:acf871f26563 27 void angle_estimator::calc_angle_phi1(int do_reset){
altb2 0:acf871f26563 28 gyro = u2gz(gz.read());
altb2 0:acf871f26563 29 if(do_reset == 1){
altb2 0:acf871f26563 30 f_ax.reset(u2ax(ax.read()));
altb2 0:acf871f26563 31 f_ay.reset(u2ax(ay.read()));
altb2 0:acf871f26563 32 f_gz.reset(gyro);
altb2 0:acf871f26563 33 }
altb2 0:acf871f26563 34 phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
altb2 0:acf871f26563 35 }
altb2 0:acf871f26563 36
altb2 0:acf871f26563 37 float angle_estimator::get_phi1(void){
altb2 0:acf871f26563 38 return phi1;
altb2 0:acf871f26563 39 }
altb2 0:acf871f26563 40 float angle_estimator::get_gyro(void){
altb2 0:acf871f26563 41 return gyro;
altb2 0:acf871f26563 42 }