fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 20:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improved tuning for current attitude estimation hack, needs to be removed ASAP

Browse Source
This commit is contained in:
Lorenz Meier 2012-08-21 17:42:24 +02:00
parent 228fbb975a
commit f3c1a7475d
1 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
View File

@ -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);