altb_pmic / AHRS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers CF_Yaw.cpp Source File

CF_Yaw.cpp

00001 #include "CF_Yaw.h"
00002 
00003 CF_Yaw::CF_Yaw(float tau, float TS)
00004 {
00005     f_mx.setup(tau,TS,1.0f);
00006     f_my.setup(tau,TS,1.0f);
00007     f_g.setup(tau,TS,tau);
00008     
00009     }
00010 CF_Yaw::~CF_Yaw() {}
00011 
00012 void CF_Yaw::update(float gz, float mx, float my, float mz)
00013 {
00014     c1 =  cosf(data.est_RPY[0]);
00015     c2 =  cosf(data.est_RPY[1]);
00016     s1 =  sinf(data.est_RPY[0]);
00017     s2 =  sinf(data.est_RPY[1]);
00018     
00019     m  << mx,my,mz;
00020     R  << c2,0,c1*s2,0,c1,-s1,-s2,c2*s1,c1*c2;
00021     m = R*m;
00022     // careful: -sign at atan2: the Magnetometer measures world frame
00023     // if copter turns CCW, the Magnet vector turns CW!!
00024     data.est_RPY[2] = uw2pi(-atan2(f_my(m(1)),f_mx(m(0))) + f_g(gz));
00025     
00026     }
Generated on Thu Jul 14 2022 22:08:52 by  doxygen 1.7.2