fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:58:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

More black magic put into the attitude estimation - works nicely now

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-29 15:53:11 +02:00
parent 7d87f2b06e
commit 7cd89520cc
4 changed files with 10 additions and 1211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+1 -1
View File
@@ -555,7 +555,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
/* add the removed length from x / y to z, since we induce a scaling issue else */
float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
accel_offset[2] = -(accel_offset[2] + total_len);
accel_offset[2] = accel_offset[2] + total_len;
if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
apps/px4/attitude_estimator_bm/attitude_bm.c
+3 -3
View File
@@ -147,9 +147,9 @@ void attitude_blackmagic_init(void)
// };
static m_elem kal_gain[12 * 9] = {
0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0,
0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0,
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+6 -6
View File
@@ -87,14 +87,14 @@ 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) * 100;
accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100;
accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100;
accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 120;
accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 120;
accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 120;
float_vect3 mag_values;
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;
mag_values.x = raw->magnetometer_ga[0]*1500.0f;
mag_values.y = raw->magnetometer_ga[1]*1500.0f;
mag_values.z = raw->magnetometer_ga[2]*1500.0f;
// static int i = 0;
apps/sensors/sensors.c
-1201
View File
File diff suppressed because it is too large Load Diff