PX4Gyroscope apply sensor rotation before scaling

- prevents loss of numerical precision

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -40,7 +40,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_gyro_pub{ORB_ID(sensor_gyro), priority},
-	_rotation{get_rot_matrix(rotation)}
+	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 
@@ -103,9 +103,14 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z
 	sensor_gyro_s &report = _sensor_gyro_pub.get();
 	report.timestamp = timestamp;
 
-	// Apply rotation, range scale, and the calibrating offset/scale
+	// Apply rotation (before scaling)
-	const matrix::Vector3f val_raw{(float)x, (float)y, (float)z};
+	float xraw_f = x;
-	const matrix::Vector3f val_calibrated{ _rotation *(((val_raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
+	float yraw_f = y;
+	float zraw_f = z;
+	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+
+	// Apply range scale and the calibrating offset/scale
+	const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))};
 
 	// Filtered values
 	const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -72,7 +72,7 @@ private:
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 	Integrator _integrator{4000, true};
 
-	const matrix::Dcmf	_rotation;
+	const enum Rotation	_rotation;
 
 	matrix::Vector3f	_calibration_scale{1.0f, 1.0f, 1.0f};
 	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};