diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-17 18:38:52 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-17 18:38:52 +0200 |
commit | 8a8b6b716530172d11e0a1b4039418e0caf11593 (patch) | |
tree | 8c0637a237999ab1a6e78dc3559cba20403df48d /apps/px4/attitude_estimator_bm | |
parent | bce043a21b7f39f786755fa3118c2c5e25eb8a94 (diff) | |
download | px4-firmware-8a8b6b716530172d11e0a1b4039418e0caf11593.tar.gz px4-firmware-8a8b6b716530172d11e0a1b4039418e0caf11593.tar.bz2 px4-firmware-8a8b6b716530172d11e0a1b4039418e0caf11593.zip |
Fixed PI wrapping code, debugging more sensor code, possible misalignment of mag and acc frames in filter
Diffstat (limited to 'apps/px4/attitude_estimator_bm')
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 16 |
1 files changed, 10 insertions, 6 deletions
diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index af7ad7187..c6869a07e 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -92,9 +92,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul accel_values.z = raw->accelerometer_m_s2[2]; float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]; - mag_values.y = raw->magnetometer_ga[1]; - mag_values.z = raw->magnetometer_ga[2]; + mag_values.x = raw->magnetometer_ga[0]*100.0f; + mag_values.y = raw->magnetometer_ga[1]*100.0f; + mag_values.z = raw->magnetometer_ga[2]*100.0f; attitude_blackmagic(&accel_values, &mag_values, &gyro_values); @@ -215,10 +215,14 @@ int attitude_estimator_bm_main(int argc, char *argv[]) att.timestamp = sensor_combined_s_local.timestamp; att.roll = euler.x; att.pitch = euler.y; - att.yaw = euler.z + M_PI; + att.yaw = euler.z - M_PI_F; - if (att.yaw > 2.0f * ((float)M_PI)) { - att.yaw -= 2.0f * ((float)M_PI); + if (att.yaw > M_PI_F) { + att.yaw -= 2.0f * M_PI_F; + } + + if (att.yaw < -M_PI_F) { + att.yaw += 2.0f * M_PI_F; } att.rollspeed = rates.x; |