fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 02:13: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 11:28:17 -05: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);
src/drivers/drv_gyro.h
+1 -4
View File
@@ -50,14 +50,11 @@
#include <uORB/topics/sensor_gyro.h>
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
/** gyro scaling factors; Vout = Vin + Voffset */
struct gyro_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
+3 -24
View File
@@ -114,11 +114,8 @@ struct {
param_t accel_z_offset;
param_t accel_z_scale;
param_t gyro_x_offset;
param_t gyro_x_scale;
param_t gyro_y_offset;
param_t gyro_y_scale;
param_t gyro_z_offset;
param_t gyro_z_scale;
param_t mag_x_offset;
param_t mag_x_scale;
param_t mag_y_offset;
@@ -245,31 +242,16 @@ void parameters_update()
PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v);
}
if (param_get(_params_handles.gyro_x_scale, &v) == 0) {
_gyro_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_x_scale %f", v);
}
if (param_get(_params_handles.gyro_y_offset, &v) == 0) {
_gyro_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v);
}
if (param_get(_params_handles.gyro_y_scale, &v) == 0) {
_gyro_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_y_scale %f", v);
}
if (param_get(_params_handles.gyro_z_offset, &v) == 0) {
_gyro_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v);
}
if (param_get(_params_handles.gyro_z_scale, &v) == 0) {
_gyro_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_z_scale %f", v);
}
// mag params
if (param_get(_params_handles.mag_x_offset, &v) == 0) {
_mag_sc.x_offset = v;
@@ -343,11 +325,8 @@ void parameters_init()
_params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE");
_params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF");
_params_handles.gyro_x_scale = param_find("CAL_GYRO0_XSCALE");
_params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF");
_params_handles.gyro_y_scale = param_find("CAL_GYRO0_YSCALE");
_params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF");
_params_handles.gyro_z_scale = param_find("CAL_GYRO0_ZSCALE");
_params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF");
_params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE");
@@ -401,9 +380,9 @@ bool create_pubs()
void update_reports()
{
_gyro.timestamp = _data.timestamp;
_gyro.x = ((_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset) * _gyro_sc.x_scale;
_gyro.y = ((_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset) * _gyro_sc.y_scale;
_gyro.z = ((_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset) * _gyro_sc.z_scale;
_gyro.x = (_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset;
_gyro.y = (_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset;
_gyro.z = (_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset;
_gyro.temperature = _data.temperature;
_gyro.scaling = _data.gyro_scaling;
_gyro.x_raw = _data.gyro_raw[0];