diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index c0c6e690c0ea13873953f47a941a49871e89d520..7c8aa430c37feb121c48b19bc687dac1de4be34d 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -767,6 +767,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 imagePayload = p.payload;
                 imageQuality = p.jpg_quality;
                 imageType = p.type;
+                imageWidth = p.width;
+                imageHeight = p.height;
                 imageStart = QGC::groundTimeMilliseconds();
             }
             break;
@@ -1392,16 +1394,13 @@ QImage UAS::getImage()
     // RAW greyscale
     if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U)
     {
-        // TODO FIXME Fabian
-        // RAW hardcoded to 22x22
-        int imgWidth = 22;
-        int imgHeight = 22;
-        int imgColors = 255;
+        // TODO FIXME
+        int imgColors = 255;//imageSize/(imageWidth*imageHeight);
         //const int headerSize = 15;
 
         // Construct PGM header
         QString header("P5\n%1 %2\n%3\n");
-        header = header.arg(imgWidth).arg(imgHeight).arg(imgColors);
+        header = header.arg(imageWidth).arg(imageHeight).arg(imgColors);
 
         QByteArray tmpImage(header.toStdString().c_str(), header.toStdString().size());
         tmpImage.append(imageRecBuffer);
@@ -1451,7 +1450,7 @@ void UAS::requestImage()
     if (imagePacketsArrived == 0)
     {
         mavlink_message_t msg;
-        mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 50);
+        mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 0, 0, 50);
         sendMessage(msg);
     }
 #endif
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 1e89ed1d4082d6c3c76409a3282f706e7ab28af9..093122db15a3754f54f54ab965c46111cb69a40f 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -214,6 +214,8 @@ protected: //COMMENTS FOR TEST UNIT
     int imagePayload;           ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases.
     int imageQuality;           ///< Quality of the transmitted image (percentage)
     int imageType;              ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit)
+    int imageWidth;             ///< Width of the image stream
+    int imageHeight;            ///< Width of the image stream
     QByteArray imageRecBuffer;  ///< Buffer for the incoming bytestream
     QImage image;               ///< Image data of last completely transmitted image
     quint64 imageStart;