gyro calibration: remove unused scale parameters

posix-configs/eagle/init/rcS

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

posix-configs/excelsior/px4.config

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

src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp

@@ -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(0) = gyro_val(0) - _gyro_calibration.x_offset;
-	gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
-	gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	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

@@ -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(0) = gyro_val(0) - _gyro_calibration.x_offset;
-	gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
-	gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	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

@@ -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 x_gyro_in_new = xraw_f - _gyro_calibration.x_offset;
-	float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	float y_gyro_in_new = yraw_f - _gyro_calibration.y_offset;
-	float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	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

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

@@ -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.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_sc.y_scale;
+	_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_sc.z_scale;
+	_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];

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

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

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

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

src/modules/commander/gyro_calibration.cpp

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

src/modules/events/temperature_calibration/gyro.cpp

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

src/modules/sensors/sensor_params_gyro0.c

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

src/modules/sensors/sensor_params_gyro1.c

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

src/modules/sensors/sensor_params_gyro2.c

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

src/modules/sensors/voted_sensors_update.cpp

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