diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-21 17:42:24 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-21 17:42:24 +0200 |
| commit | f3c1a7475d0526501a340b99aee5c5d4d91300a1 (patch) | |
| tree | ec3d331d67907f38734af15449d22475ea4b0e72 | |
| parent | 228fbb975aa2d97395dd9b34937b1348ea6b3230 (diff) | |
| download | px4-firmware-f3c1a7475d0526501a340b99aee5c5d4d91300a1.tar.gz px4-firmware-f3c1a7475d0526501a340b99aee5c5d4d91300a1.tar.bz2 px4-firmware-f3c1a7475d0526501a340b99aee5c5d4d91300a1.zip | |
Improved tuning for current attitude estimation hack, needs to be removed ASAP
| -rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 23 |
1 files changed, 17 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 feaed6695..e56ff1c3a 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,14 +87,25 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul gyro_values.z = raw->gyro_rad_s[2]; float_vect3 accel_values; - accel_values.x = raw->accelerometer_m_s2[0] * 9.81f * 9.0f; - accel_values.y = raw->accelerometer_m_s2[1] * 9.81f * 9.0f; - accel_values.z = raw->accelerometer_m_s2[2] * 9.81f * 9.0f; + accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; + accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; + accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*510.0f; - mag_values.y = raw->magnetometer_ga[1]*510.0f; - mag_values.z = raw->magnetometer_ga[2]*510.0f; + mag_values.x = raw->magnetometer_ga[0]*456.0f; + mag_values.y = raw->magnetometer_ga[1]*456.0f; + mag_values.z = raw->magnetometer_ga[2]*456.0f; + + static int i = 0; + + if (i == 500) { + printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", + gyro_values.x, gyro_values.y, gyro_values.z, + accel_values.x, accel_values.y, accel_values.z, + mag_values.x, mag_values.y, mag_values.z); + i = 0; + } + i++; attitude_blackmagic(&accel_values, &mag_values, &gyro_values); |