fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
posix-configs/eagle/init/rcS
View File

@ -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

3
posix-configs/excelsior/px4.config
View File

@ -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

33
src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
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,

33
src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
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,

33
src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
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);

5
src/drivers/drv_gyro.h
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;
};
/*

27
src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
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];

5
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -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;
@ -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)};
@ -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));

1
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -77,7 +77,6 @@ private:
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};

32
src/modules/commander/gyro_calibration.cpp
View File

@ -217,14 +217,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
worker_data.mavlink_log_pub = mavlink_log_pub;
struct gyro_calibration_s gyro_scale_zero;
gyro_scale_zero.x_offset = 0.0f;
gyro_scale_zero.x_scale = 1.0f;
gyro_scale_zero.y_offset = 0.0f;
gyro_scale_zero.y_scale = 1.0f;
gyro_scale_zero.z_offset = 0.0f;
gyro_scale_zero.z_scale = 1.0f;
gyro_calibration_s gyro_scale_zero{};
int device_prio_max = 0;
int32_t device_id_primary = 0;
@ -245,7 +238,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
// Reset all offsets to 0 and scales to 1
// Reset all offsets to 0
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
@ -284,27 +277,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif

4
src/modules/events/temperature_calibration/gyro.cpp
View File

@ -60,15 +60,11 @@ void TemperatureCalibrationGyro::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
float scale = 1.0f;
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_GYRO%u_XOFF", s, &offset);
set_parameter("CAL_GYRO%u_YOFF", s, &offset);
set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
set_parameter("CAL_GYRO%u_XSCALE", s, &scale);
set_parameter("CAL_GYRO%u_YSCALE", s, &scale);
set_parameter("CAL_GYRO%u_ZSCALE", s, &scale);
}
}

23
src/modules/sensors/sensor_params_gyro0.c
View File

@ -72,26 +72,3 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO0_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO0_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f);

23
src/modules/sensors/sensor_params_gyro1.c
View File

@ -72,26 +72,3 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f);

23
src/modules/sensors/sensor_params_gyro2.c
View File

@ -72,26 +72,3 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f);

12
src/modules/sensors/voted_sensors_update.cpp
View File

@ -265,18 +265,6 @@ void VotedSensorsUpdate::parametersUpdate()
failed = failed || (OK != param_get(param_find(str), &gscale.z_offset));
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_scale));
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.y_scale));
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);