altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

CF_Yaw.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
22:495a419e474c 
Correct magnetometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 22:495a419e474c 1 #include "CF_Yaw.h"
altb2 22:495a419e474c 2
altb2 22:495a419e474c 3 CF_Yaw::CF_Yaw(float tau, float TS)
altb2 22:495a419e474c 4 {
altb2 22:495a419e474c 5 f_mx.setup(tau,TS,1.0f);
altb2 22:495a419e474c 6 f_my.setup(tau,TS,1.0f);
altb2 22:495a419e474c 7 f_g.setup(tau,TS,tau);
altb2 22:495a419e474c 8
altb2 22:495a419e474c 9 }
altb2 22:495a419e474c 10 CF_Yaw::~CF_Yaw() {}
altb2 22:495a419e474c 11
altb2 22:495a419e474c 12 void CF_Yaw::update(float gz, float mx, float my, float mz)
altb2 22:495a419e474c 13 {
altb2 22:495a419e474c 14 c1 = cosf(data.est_RPY[0]);
altb2 22:495a419e474c 15 c2 = cosf(data.est_RPY[1]);
altb2 22:495a419e474c 16 s1 = sinf(data.est_RPY[0]);
altb2 22:495a419e474c 17 s2 = sinf(data.est_RPY[1]);
altb2 22:495a419e474c 18
altb2 22:495a419e474c 19 m << mx,my,mz;
altb2 22:495a419e474c 20 R << c2,0,c1*s2,0,c1,-s1,-s2,c2*s1,c1*c2;
altb2 22:495a419e474c 21 m = R*m;
altb2 22:495a419e474c 22 // careful: -sign at atan2: the Magnetometer measures world frame
altb2 22:495a419e474c 23 // if copter turns CCW, the Magnet vector turns CW!!
altb2 22:495a419e474c 24 data.est_RPY[2] = uw2pi(-atan2(f_my(m(1)),f_mx(m(0))) + f_g(gz));
altb2 22:495a419e474c 25
altb2 22:495a419e474c 26 }