From 2a5fcd917428fa6e549214f8066690672b453af0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Aug 2012 09:35:58 +0200 Subject: [PATCH] Fixed incorrect scaling of acceleration values --- apps/mavlink/mavlink.c | 4 ++-- apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 6 +++--- apps/sensors/sensors.c | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index cbea7822e4..b6fdc1a3e9 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -635,8 +635,8 @@ static void *uorb_receiveloop(void *arg) /* send raw imu data */ mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); - /* send scaled imu data */ - mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 9810, buf.raw.accelerometer_m_s2[1] * 9810, buf.raw.accelerometer_m_s2[2] * 9810, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000); + /* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */ + mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000); /* send pressure */ mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 7385171a29..c917ff7b26 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,9 +87,9 @@ 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]; - accel_values.y = raw->accelerometer_m_s2[1]; - accel_values.z = raw->accelerometer_m_s2[2]; + accel_values.x = raw->accelerometer_m_s2[0] * 9.81f; + accel_values.y = raw->accelerometer_m_s2[1] * 9.81f; + accel_values.z = raw->accelerometer_m_s2[2] * 9.81f; float_vect3 mag_values; mag_values.x = raw->magnetometer_ga[0]*100.0f; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 89a6d9c755..564ee4f8c4 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -819,9 +819,9 @@ int sensors_thread_main(int argc, char *argv[]) // XXX read range from sensor float range_g = 4.0f; /* scale from 14 bit to m/s2 */ - raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) / 8192.0f) * range_g) * 9.81f; + raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) / 8192.0f) * range_g) * 9.81f; + raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) / 8192.0f) * range_g) * 9.81f; raw.accelerometer_raw_counter++; }