AHRS
Dependencies: Eigen
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?
User | Revision | Line number | New 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 | } |