rotate accel/gyro FIFO before publish and fix angular velocity filter resets

- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
     - to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
 - fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
      - in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)

src/lib/sensor_calibration/Gyroscope.hpp

@@ -82,6 +82,11 @@ public:
 		return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
 	}
 
+	inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
+	{
+		return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
+	}
+
 	bool ParametersSave();
 	void ParametersUpdate();