diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-27 15:44:50 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-27 15:44:50 +0200 |
commit | 97d9e67e67014a1eb2dccd08f35906e5ba1a5f84 (patch) | |
tree | dc34a82890dbc0de2c36b8d06a55ce3855ea59b3 | |
parent | 967c0869a962f9288a54926b5eec9587f9c11df3 (diff) | |
download | px4-firmware-97d9e67e67014a1eb2dccd08f35906e5ba1a5f84.tar.gz px4-firmware-97d9e67e67014a1eb2dccd08f35906e5ba1a5f84.tar.bz2 px4-firmware-97d9e67e67014a1eb2dccd08f35906e5ba1a5f84.zip |
Really ugly but safe hack to make 30 deg rotations work
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.c | 12 |
1 files changed, 12 insertions, 0 deletions
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c index 1ffe9f7bd..cb17a356d 100644 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -221,6 +221,18 @@ void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const m_elem mask[9] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; + // XXX Hack - stop updating accel if upside down + + if (accel->z > 0) { + mask[0] = 0.0f; + mask[1] = 0.0f; + mask[2] = 0.0f; + } else { + mask[0] = 1.0f; + mask[1] = 1.0f; + mask[2] = 1.0f; + } + measurement[0] = accel->x; measurement[1] = accel->y; measurement[2] = accel->z; |