fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed PI wrapping code, debugging more sensor code, possible misalignment of mag and acc frames in filter

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-17 18:38:52 +02:00
parent bce043a21b
commit 8a8b6b7165
2 changed files with 16 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+10 -6
View File
@@ -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;
apps/sensors/sensors.c
+6 -6
View File
@@ -315,7 +315,7 @@ int sensors_main(int argc, char *argv[])
float buf_barometer[3];
int16_t mag_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {200, 0, 0};
int16_t gyro_offset[3] = {0, 0, 0};
bool mag_calibration_enabled = false;
@@ -781,10 +781,10 @@ int sensors_main(int argc, char *argv[])
if (gyro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor
/* scale measurements */
// XXX request scaling from driver instead of hardcoding it
@@ -802,7 +802,7 @@ int sensors_main(int argc, char *argv[])
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32768 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
@@ -820,8 +820,8 @@ int sensors_main(int argc, char *argv[])
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is -y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is x of the sensor
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is -x of the sensor
raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here