Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Speed up gyro calibration #13632

Merged
merged 3 commits into from
Nov 29, 2019
Merged

Speed up gyro calibration #13632

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d2fccf3
gyro calibration: speedup from 20s to 1s
bkueng Nov 28, 2019
9a36899
gyro calibration: slightly stricter motion detection check
bkueng Nov 28, 2019
561e6ca
gyro calibration: remove unused scale parameters
bkueng Nov 29, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions posix-configs/eagle/init/rcS
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,9 +141,6 @@ then
param set CAL_GYRO0_XOFF -0.0028
param set CAL_GYRO0_YOFF -0.0047
param set CAL_GYRO0_ZOFF -0.0034
param set CAL_GYRO0_XSCALE 1.0000
param set CAL_GYRO0_YSCALE 1.0000
param set CAL_GYRO0_ZSCALE 1.0000
param set CAL_MAG0_XOFF 0.0000
param set CAL_MAG0_YOFF 0.0000
param set CAL_MAG0_ZOFF 0.0000
Expand Down
3 changes: 0 additions & 3 deletions posix-configs/excelsior/px4.config
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,6 @@ param set SENS_BOARD_ROT 4
param set CAL_GYRO0_XOFF -0.0028
param set CAL_GYRO0_YOFF -0.0047
param set CAL_GYRO0_ZOFF -0.0034
param set CAL_GYRO0_XSCALE 1.0000
param set CAL_GYRO0_YSCALE 1.0000
param set CAL_GYRO0_ZSCALE 1.0000
param set CAL_MAG0_XOFF 0.0000
param set CAL_MAG0_YOFF 0.0000
param set CAL_MAG0_ZOFF 0.0000
Expand Down
33 changes: 3 additions & 30 deletions src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand Down
33 changes: 3 additions & 30 deletions src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand Down
33 changes: 3 additions & 30 deletions src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
5 changes: 1 addition & 4 deletions src/drivers/drv_gyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

/*
Expand Down
27 changes: 3 additions & 24 deletions src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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];
Expand Down
5 changes: 1 addition & 4 deletions src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,6 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal));

_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
_calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
}

return PX4_OK;
Expand Down Expand Up @@ -118,7 +117,7 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
const matrix::Vector3f raw{x, y, z};

// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
const matrix::Vector3f val_calibrated{((raw * report.scaling) - _calibration_offset)};

// Filtered values
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
Expand Down Expand Up @@ -176,8 +175,6 @@ PX4Gyroscope::print_status()
PX4_INFO("sample rate: %d Hz", _sample_rate);
PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq());

PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
(double)_calibration_scale(2));
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));

Expand Down
1 change: 0 additions & 1 deletion src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,6 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams

const enum Rotation _rotation;

matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};

int _class_device_instance{-1};
Expand Down
Loading