fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 16:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro calibration: remove unused scale parameters

Browse Source
This commit is contained in:
Beat Küng
2019-11-29 10:33:19 +01:00
committed by Daniel Agar
parent 4ca5770f36
commit 5ff83ef740
15 changed files with 16 additions and 244 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
+3 -30
View File
@@ -204,9 +204,6 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -345,27 +342,6 @@ void DfLsm9ds1Wrapper::_update_gyro_calibration()
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
@@ -391,9 +367,6 @@ void DfLsm9ds1Wrapper::_update_gyro_calibration()
return;
}
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -590,9 +563,9 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation
gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset;
gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset;
_gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val,
vec_integrated_unused,
src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
+3 -30
View File
@@ -186,9 +186,6 @@ DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) :
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -286,27 +283,6 @@ void DfMPU6050Wrapper::_update_gyro_calibration()
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
@@ -332,9 +308,6 @@ void DfMPU6050Wrapper::_update_gyro_calibration()
return;
}
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -454,9 +427,9 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset;
gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset;
_gyro_int.put(now,
gyro_val,
src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+3 -30
View File
@@ -224,9 +224,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -387,27 +384,6 @@ void DfMpu9250Wrapper::_update_gyro_calibration()
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
res = param_get(param_find(str), &_gyro_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
@@ -433,9 +409,6 @@ void DfMpu9250Wrapper::_update_gyro_calibration()
return;
}
_gyro_calibration.x_scale = 1.0f;
_gyro_calibration.y_scale = 1.0f;
_gyro_calibration.z_scale = 1.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
@@ -668,9 +641,9 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
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;
float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
float x_gyro_in_new = xraw_f - _gyro_calibration.x_offset;
float y_gyro_in_new = yraw_f - _gyro_calibration.y_offset;
float z_gyro_in_new = zraw_f - _gyro_calibration.z_offset;
gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new);
gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);