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 | |
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
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 16 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 12 |
2 files changed, 16 insertions, 12 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; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 7933e8518..aa006f3a6 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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 |