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

drivers replace math::Vector<3> with matrix::Vector3f

Browse Source
This commit is contained in:
Daniel Agar
2018-03-21 13:48:45 -04:00
parent e504dc86b7
commit 1b174eeca2
20 changed files with 97 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
+3 -3
View File
@@ -111,7 +111,7 @@ private:
float z_scale;
} _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
matrix::Dcmf _rotation_matrix;
int _mag_orb_class_instance;
@@ -136,7 +136,7 @@ DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) :
_mag_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
_rotation_matrix = get_rot_matrix(rotation);
}
DfAK8963Wrapper::~DfAK8963Wrapper()
@@ -285,7 +285,7 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
mag_report.y_raw = 0;
mag_report.z_raw = 0;
math::Vector<3> mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga);
matrix::Vector3f mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga);
// apply sensor rotation on the accel measurement
mag_val = _rotation_matrix * mag_val;
src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+5 -5
View File
@@ -111,7 +111,7 @@ private:
float z_scale;
} _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
matrix::Dcmf _rotation_matrix;
int _mag_orb_class_instance;
@@ -136,7 +136,7 @@ DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) :
_mag_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
_rotation_matrix = get_rot_matrix(rotation);
}
DfHmc5883Wrapper::~DfHmc5883Wrapper()
@@ -283,9 +283,9 @@ int DfHmc5883Wrapper::_publish(struct mag_sensor_data &data)
mag_report.y_raw = 0;
mag_report.z_raw = 0;
math::Vector<3> mag_val(data.field_x_ga,
data.field_y_ga,
data.field_z_ga);
matrix::Vector3f mag_val(data.field_x_ga,
data.field_y_ga,
data.field_z_ga);
// apply sensor rotation on the accel measurement
mag_val = _rotation_matrix * mag_val;
src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
+16 -16
View File
@@ -145,7 +145,7 @@ private:
float z_scale;
} _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
matrix::Dcmf _rotation_matrix;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
int _mag_orb_class_instance;
@@ -221,7 +221,7 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
}
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
_rotation_matrix = get_rot_matrix(rotation);
}
DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper()
@@ -569,11 +569,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
_update_gyro_calibration();
}
math::Vector<3> vec_integrated_unused;
matrix::Vector3f vec_integrated_unused;
uint64_t integral_dt_unused;
math::Vector<3> accel_val(data.accel_m_s2_x,
data.accel_m_s2_y,
data.accel_m_s2_z);
matrix::Vector3f accel_val(data.accel_m_s2_x,
data.accel_m_s2_y,
data.accel_m_s2_z);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
// Apply calibration after rotation
@@ -584,9 +584,9 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_val,
vec_integrated_unused,
integral_dt_unused);
math::Vector<3> gyro_val(data.gyro_rad_s_x,
data.gyro_rad_s_y,
data.gyro_rad_s_z);
matrix::Vector3f gyro_val(data.gyro_rad_s_x,
data.gyro_rad_s_y,
data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation
@@ -656,12 +656,12 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
mag_report.z_raw = 0;
}
math::Vector<3> gyro_val_filt;
math::Vector<3> accel_val_filt;
matrix::Vector3f gyro_val_filt;
matrix::Vector3f accel_val_filt;
// Read and reset.
math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
@@ -673,9 +673,9 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.z = accel_val_filt(2);
if (_mag_enabled) {
math::Vector<3> mag_val(data.mag_ga_x,
data.mag_ga_y,
data.mag_ga_z);
matrix::Vector3f mag_val(data.mag_ga_x,
data.mag_ga_y,
data.mag_ga_z);
mag_val = _rotation_matrix * mag_val;
mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
+9 -9
View File
@@ -131,7 +131,7 @@ private:
float z_scale;
} _gyro_calibration;
math::Matrix<3, 3> _rotation_matrix;
matrix::Dcmf _rotation_matrix;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
@@ -193,7 +193,7 @@ DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) :
_gyro_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
_rotation_matrix = get_rot_matrix(rotation);
}
DfMPU6050Wrapper::~DfMPU6050Wrapper()
@@ -431,10 +431,10 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
uint64_t now = hrt_absolute_time();
math::Vector<3> vec_integrated_unused;
matrix::Vector3f vec_integrated_unused;
uint64_t integral_dt_unused;
math::Vector<3> accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
@@ -448,7 +448,7 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
vec_integrated_unused,
integral_dt_unused);
math::Vector<3> gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
@@ -511,12 +511,12 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
accel_report.y_raw = 0;
accel_report.z_raw = 0;
math::Vector<3> gyro_val_filt;
math::Vector<3> accel_val_filt;
matrix::Vector3f gyro_val_filt;
matrix::Vector3f accel_val_filt;
// Read and reset.
math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+4 -4
View File
@@ -620,8 +620,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f aval_integrated;
_accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
accel_report.x_integral = aval_integrated(0);
@@ -652,8 +652,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);
gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
matrix::Vector3f gval_integrated;
_gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
gyro_report.x_integral = gval_integrated(0);
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+4 -4
View File
@@ -1050,8 +1050,8 @@ GYROSIM::_measure()
arb.y = mpu_report.accel_y;
arb.z = mpu_report.accel_z;
math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z);
math::Vector<3> aval_integrated;
matrix::Vector3f aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z);
matrix::Vector3f aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
@@ -1075,8 +1075,8 @@ GYROSIM::_measure()
grb.y = mpu_report.gyro_y;
grb.z = mpu_report.gyro_z;
math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z);
math::Vector<3> gval_integrated;
matrix::Vector3f gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);