diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-19 09:35:58 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-19 09:35:58 +0200 |
| commit | 2a5fcd917428fa6e549214f8066690672b453af0 (patch) | |
| tree | 82dc2b45a75acd17a13a45831b319bef8e658a06 | |
| parent | dcf71d5f69e11335d91d749d740a0c9fafb05586 (diff) | |
| download | px4-firmware-2a5fcd917428fa6e549214f8066690672b453af0.tar.gz px4-firmware-2a5fcd917428fa6e549214f8066690672b453af0.tar.bz2 px4-firmware-2a5fcd917428fa6e549214f8066690672b453af0.zip | |
Fixed incorrect scaling of acceleration values
| -rw-r--r-- | apps/mavlink/mavlink.c | 4 | ||||
| -rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 6 | ||||
| -rw-r--r-- | 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 cbea7822e..b6fdc1a3e 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 7385171a2..c917ff7b2 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 89a6d9c75..564ee4f8c 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++; } |