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

Stream scaled IMU for Snapdragon Flight using VISLAM

Browse Source 
This is temporary (and for Snapdragon Flight + VISLAM only) until there is a proper solution to get unfiltered IMU data for VIOs etc.
This commit is contained in:
ChristophTobler
2018-01-05 18:45:50 +01:00
committed by Lorenz Meier
parent 545f8c4452
commit 3ffc1fd25b
2 changed files with 108 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+18 -14
View File
@@ -599,11 +599,6 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// ACCEL
// write raw data (without rotation)
accel_report.x_raw = data.accel_m_s2_x;
accel_report.y_raw = data.accel_m_s2_y;
accel_report.z_raw = data.accel_m_s2_z;
float xraw_f = data.accel_m_s2_x;
float yraw_f = data.accel_m_s2_y;
float zraw_f = data.accel_m_s2_z;
@@ -611,6 +606,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];
// adjust values according to the calibration
float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
@@ -630,11 +631,6 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// GYRO
// write raw data (withoud rotation)
gyro_report.x_raw = data.gyro_rad_s_x;
gyro_report.y_raw = data.gyro_rad_s_y;
gyro_report.z_raw = data.gyro_rad_s_z;
xraw_f = data.gyro_rad_s_x;
yraw_f = data.gyro_rad_s_y;
zraw_f = data.gyro_rad_s_z;
@@ -642,6 +638,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];
// adjust values according to the calibration
float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
@@ -706,16 +708,18 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
mag_report.range_ga = -1.0f;
mag_report.device_id = m_id.dev_id;
mag_report.x_raw = 0;
mag_report.y_raw = 0;
mag_report.z_raw = 0;
xraw_f = data.mag_ga_x;
yraw_f = data.mag_ga_y;
zraw_f = data.mag_ga_z;
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]
mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;