getPhoto: save shot to photo.jpg + fixed yuv422p conversion

trikset · Jan 25, 2018 · 125ab9b · 125ab9b
1 parent 83aba12
commit 125ab9b
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 15 deletions.
diff --git a/trikControl/src/v4l2CameraImplementation.cpp b/trikControl/src/v4l2CameraImplementation.cpp
@@ -1,5 +1,6 @@
+#include <QImage>
 #include "v4l2CameraImplementation.h"
-
+#include "QsLog.h"
 using namespace trikControl;
 
 V4l2CameraImplementation::V4l2CameraImplementation(const QString &port, trikHal::HardwareAbstractionInterface &hardwareAbstraction)
@@ -10,5 +11,14 @@ V4l2CameraImplementation::V4l2CameraImplementation(const QString &port, trikHal:
 
 QVector<uint8_t> V4l2CameraImplementation::getPhoto()
 {
-	return mHal.captureV4l2StillImage(mPort, getTempDir());
+	auto result = mHal.captureV4l2StillImage(mPort, getTempDir());
+	if (result.empty())
+		return result;
+
+	auto image = QImage(result.data(), 320, 240, QImage::Format_RGB888);
+	auto saved = image.save(getTempDir()+"/photo.jpg", "JPG");
+	if (!saved) {
+		QLOG_WARN() << "Failed to save captured image";
+	}
+	return result;
 }
diff --git a/trikHal/src/trik/trikHardwareAbstraction.cpp b/trikHal/src/trik/trikHardwareAbstraction.cpp
@@ -74,7 +74,7 @@ OutputDeviceFileInterface *TrikHardwareAbstraction::createOutputDeviceFile(const
 	return new TrikOutputDeviceFile(fileName);
 }
 
-inline int clip255(int x) { return x > 255 ? 255 : x; }
+inline unsigned clip255(int x) { return x >= 255 ? 255 : (x <= 0)? 0 : x; }
 
 QVector<uint8_t> TrikHardwareAbstraction::captureV4l2StillImage(const QString &port, const QString &pathToPic) const
 {
@@ -83,34 +83,38 @@ QVector<uint8_t> TrikHardwareAbstraction::captureV4l2StillImage(const QString &p
 
 	QLOG_INFO() << "Start open v4l2 device" << port;
 
-	auto shot =  device.makeShot();
+	const QVector<uint8_t> & shot =  device.makeShot();
 
 	QLOG_INFO() << "End capturing v4l2 from port" << port << " with " << shot.size() << "bytes";
 
 	QVector<uint8_t> result(shot.size()/4*2*3); // YUV422 to RGB888
 	constexpr auto IMAGE_WIDTH = 320;
 	const auto IMAGE_HEIGHT = shot.length() / IMAGE_WIDTH / 2;
+	const auto Y = &shot[0];
+	const auto U = &shot[IMAGE_WIDTH*IMAGE_HEIGHT];
+	const auto V = &shot[3*IMAGE_WIDTH*IMAGE_HEIGHT/2];
 	for (auto row = 0; row < IMAGE_HEIGHT; ++row) {
-		for (auto col = 0; col < IMAGE_WIDTH; col += 4) {
+		for (auto col = 0; col < IMAGE_WIDTH; col+=2) {
 			auto startIndex = row * IMAGE_WIDTH + col;
-			auto yuv422 = &shot[startIndex];
-			auto y1 = yuv422[0] - 16;
-			auto u  = yuv422[1] - 128;
-			auto y2 = yuv422[2] - 16;
-			auto v  = yuv422[3] - 128;
+			auto y1 = Y[startIndex] - 16;
+			auto y2 = Y[startIndex+1] - 16;
+			auto u  = U[startIndex>>1] - 128;
+			auto v  = V[startIndex>>1] - 128;
 			auto _298y1 = 298 * y1;
 			auto _298y2 = 298 * y2;
 			auto _409v  = 409 * v;
 			auto _100u  = -100 * u;
 			auto _516u  = 516 * u;
 			auto _208v  = -208 * v;
-			auto r1 = clip255 ((_298y1 + _409v + 128)>>8);
+			auto r1 = clip255 ((_298y1 + _409v + 128) >> 8);
 			auto g1 = clip255 ((_298y1 + _100u + _208v + 128) >> 8);
-			auto b1 = clip255 ((_298y1 + _516u + 128)>> 8);
-			auto r2 = clip255 ((_298y2 + _409v + 128)>>8);
+			auto b1 = clip255 ((_298y1 + _516u + 128) >> 8);
+			auto r2 = clip255 ((_298y2 + _409v + 128) >> 8);
 			auto g2 = clip255 ((_298y2 + _100u + _208v + 128) >> 8);
-			auto b2 = clip255 ((_298y2 + _516u + 128)>> 8);
-			auto rgb = &result[startIndex/4*2*3];
+			auto b2 = clip255 ((_298y2 + _516u + 128) >> 8);
+
+
+			auto rgb = &result[startIndex*3];
 			rgb[0] = r1;
 			rgb[1] = g1;
 			rgb[2] = b1;
@@ -119,6 +123,7 @@ QVector<uint8_t> TrikHardwareAbstraction::captureV4l2StillImage(const QString &p
 			rgb[5] = b2;
 		}
 	}
+	QLOG_INFO() << "Captrured RGB888 " << result.size() << "bytes image";
 	return result;
 }