Skip to content

Q
qgroundcontrol
Commit 178bc32c authored by LM's avatar LM
Browse files
Options

Fixed image transmission code

parent 919c2b20
Show whitespace changes
Inline Side-by-side
Showing with 776 additions and 737 deletions
src/uas/UAS.cc
View file @ 178bc32c
...@@ -28,53 +28,53 @@ ...@@ -28,53 +28,53 @@
#include "SerialLink.h" #include "SerialLink.h"
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
uasId(id), uasId(id),
startTime(QGC::groundTimeMilliseconds()), startTime(QGC::groundTimeMilliseconds()),
commStatus(COMM_DISCONNECTED), commStatus(COMM_DISCONNECTED),
name(""), name(""),
autopilot(-1), autopilot(-1),
links(new QList<LinkInterface*>()), links(new QList<LinkInterface*>()),
unknownPackets(), unknownPackets(),
mavlink(protocol), mavlink(protocol),
waypointManager(*this), waypointManager(*this),
thrustSum(0), thrustSum(0),
thrustMax(10), thrustMax(10),
startVoltage(0), startVoltage(0),
warnVoltage(9.5f), warnVoltage(9.5f),
warnLevelPercent(20.0f), warnLevelPercent(20.0f),
currentVoltage(12.0f), currentVoltage(12.0f),
lpVoltage(12.0f), lpVoltage(12.0f),
batteryRemainingEstimateEnabled(false), batteryRemainingEstimateEnabled(false),
mode(-1), mode(-1),
status(-1), status(-1),
navMode(-1), navMode(-1),
onboardTimeOffset(0), onboardTimeOffset(0),
controlRollManual(true), controlRollManual(true),
controlPitchManual(true), controlPitchManual(true),
controlYawManual(true), controlYawManual(true),
controlThrustManual(true), controlThrustManual(true),
manualRollAngle(0), manualRollAngle(0),
manualPitchAngle(0), manualPitchAngle(0),
manualYawAngle(0), manualYawAngle(0),
manualThrust(0), manualThrust(0),
receiveDropRate(0), receiveDropRate(0),
sendDropRate(0), sendDropRate(0),
lowBattAlarm(false), lowBattAlarm(false),
positionLock(false), positionLock(false),
localX(0.0), localX(0.0),
localY(0.0), localY(0.0),
localZ(0.0), localZ(0.0),
latitude(0.0), latitude(0.0),
longitude(0.0), longitude(0.0),
altitude(0.0), altitude(0.0),
roll(0.0), roll(0.0),
pitch(0.0), pitch(0.0),
yaw(0.0), yaw(0.0),
statusTimeout(new QTimer(this)), statusTimeout(new QTimer(this)),
paramsOnceRequested(false), paramsOnceRequested(false),
airframe(0), airframe(0),
attitudeKnown(false), attitudeKnown(false),
paramManager(NULL) paramManager(NULL)
{ {
color = UASInterface::getNextColor(); color = UASInterface::getNextColor();
setBattery(LIPOLY, 3); setBattery(LIPOLY, 3);
...@@ -877,6 +877,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -877,6 +877,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
imagePackets = p.packets; imagePackets = p.packets;
imagePayload = p.payload; imagePayload = p.payload;
imageQuality = p.jpg_quality; imageQuality = p.jpg_quality;
imageType = p.type;
imageStart = QGC::groundTimeMilliseconds(); imageStart = QGC::groundTimeMilliseconds();
} }
break; break;
...@@ -887,6 +888,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -887,6 +888,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
int seq = img.seqnr; int seq = img.seqnr;
int pos = seq * imagePayload; int pos = seq * imagePayload;
// Check if we have a valid transaction
if (imagePackets == 0)
{
// NO VALID TRANSACTION - ABORT
// Restart statemachine
imagePacketsArrived = 0;
}
for (int i = 0; i < imagePayload; ++i) { for (int i = 0; i < imagePayload; ++i) {
if (pos <= imageSize) { if (pos <= imageSize) {
imageRecBuffer[pos] = img.data[i]; imageRecBuffer[pos] = img.data[i];
...@@ -897,15 +906,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -897,15 +906,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
++imagePacketsArrived; ++imagePacketsArrived;
// emit signal if all packets arrived // emit signal if all packets arrived
if ((imagePacketsArrived == imagePackets)) if ((imagePacketsArrived >= imagePackets))
{ {
// Restart statemachine // Restart statemachine
imagePacketsArrived = 0; imagePacketsArrived = 0;
emit imageReady(this); emit imageReady(this);
qDebug() << "imageReady emitted. all packets arrived"; qDebug() << "imageReady emitted. all packets arrived";
//this->requestImage();
//qDebug() << "SENDING REQUEST TO GET NEW IMAGE FROM SYSTEM" << uasId;
} }
} }
break; break;
...@@ -1120,7 +1126,7 @@ void UAS::startPressureCalibration() ...@@ -1120,7 +1126,7 @@ void UAS::startPressureCalibration()
quint64 UAS::getUnixTime(quint64 time) quint64 UAS::getUnixTime(quint64 time)
{ {
if (time == 0) { if (time == 0) {
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds(); // qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds(); return QGC::groundTimeMilliseconds();
} }
// Check if time is smaller than 40 years, // Check if time is smaller than 40 years,
...@@ -1145,7 +1151,7 @@ quint64 UAS::getUnixTime(quint64 time) ...@@ -1145,7 +1151,7 @@ quint64 UAS::getUnixTime(quint64 time)
else if (time < 1261440000000000) else if (time < 1261440000000000)
#endif #endif
{ {
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
if (onboardTimeOffset == 0) { if (onboardTimeOffset == 0) {
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
} }
...@@ -1326,12 +1332,58 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc ...@@ -1326,12 +1332,58 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
QImage UAS::getImage() QImage UAS::getImage()
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK #ifdef MAVLINK_ENABLED_PIXHAWK
image.loadFromData(imageRecBuffer);
qDebug() << "IMAGE TYPE:" << imageType;
// 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;
//const int headerSize = 15;
// Construct PGM header
QString header("P5\n%1 %2\n%3\n");
header = header.arg(imgWidth).arg(imgHeight).arg(imgColors);
QByteArray tmpImage(header.toStdString().c_str(), header.toStdString().size());
tmpImage.append(imageRecBuffer);
//qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header;
if (imageRecBuffer.isNull())
{
qDebug()<< "could not convertToPGM()";
return QImage();
}
if (!image.loadFromData(tmpImage, "PGM"))
{
qDebug()<< "could not create extracted image";
return QImage();
}
}
// BMP with header
else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
imageType == MAVLINK_DATA_STREAM_IMG_PGM ||
imageType == MAVLINK_DATA_STREAM_IMG_PNG)
{
if (!image.loadFromData(imageRecBuffer))
{
qDebug() << "Loading data from image buffer failed!";
}
}
// Restart statemachine // Restart statemachine
imagePacketsArrived = 0; imagePacketsArrived = 0;
//imageRecBuffer.clear();
return image; return image;
#endif #endif
} }
...@@ -1648,11 +1700,11 @@ void UAS::setParameter(const int component, const QString& id, const float value ...@@ -1648,11 +1700,11 @@ void UAS::setParameter(const int component, const QString& id, const float value
if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) { if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) {
p.param_id[i] = id.toAscii()[i]; p.param_id[i] = id.toAscii()[i];
} }
// // Null termination at end of string or end of buffer // // Null termination at end of string or end of buffer
// else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1)) // else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
// { // {
// p.param_id[i] = '\0'; // p.param_id[i] = '\0';
// } // }
// Zero fill // Zero fill
else { else {
p.param_id[i] = 0; p.param_id[i] = 0;
......
src/uas/UAS.h
View file @ 178bc32c
...@@ -187,7 +187,8 @@ protected: //COMMENTS FOR TEST UNIT ...@@ -187,7 +187,8 @@ protected: //COMMENTS FOR TEST UNIT
int imagePackets; ///< Number of data packets being sent for this image int imagePackets; ///< Number of data packets being sent for this image
int imagePacketsArrived; ///< Number of data packets recieved int imagePacketsArrived; ///< Number of data packets recieved
int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases.
int imageQuality; ///< JPEG-Quality of the transmitted image (percentage) int imageQuality; ///< Quality of the transmitted image (percentage)
int imageType; ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit)
QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream
QImage image; ///< Image data of last completely transmitted image QImage image; ///< Image data of last completely transmitted image
quint64 imageStart; quint64 imageStart;
......
src/ui/HUD.cc
View file @ 178bc32c
...@@ -106,7 +106,6 @@ HUD::HUD(int width, int height, QWidget* parent) ...@@ -106,7 +106,6 @@ HUD::HUD(int width, int height, QWidget* parent)
fuelColor(criticalColor), fuelColor(criticalColor),
warningBlinkRate(5), warningBlinkRate(5),
refreshTimer(new QTimer(this)), refreshTimer(new QTimer(this)),
imageTimer(new QTimer(this)),
noCamera(true), noCamera(true),
hardwareAcceleration(true), hardwareAcceleration(true),
strongStrokeWidth(1.5f), strongStrokeWidth(1.5f),
...@@ -161,14 +160,11 @@ HUD::HUD(int width, int height, QWidget* parent) ...@@ -161,14 +160,11 @@ HUD::HUD(int width, int height, QWidget* parent)
//qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath; //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
//fill = QImage(imagePath); //fill = QImage(imagePath);
//glImage = QGLWidget::convertToGLFormat(fill); glImage = QGLWidget::convertToGLFormat(fill);
// Refresh timer // Refresh timer
refreshTimer->setInterval(updateInterval); refreshTimer->setInterval(updateInterval);
imageTimer->setInterval(250);
//connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update()));
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD())); connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
// connect(imageTimer, SIGNAL(timeout()), this, SLOT(requestNewImage())); TODO
// Resize to correct size and fill with image // Resize to correct size and fill with image
//glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits()); //glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
...@@ -207,7 +203,6 @@ HUD::HUD(int width, int height, QWidget* parent) ...@@ -207,7 +203,6 @@ HUD::HUD(int width, int height, QWidget* parent)
HUD::~HUD() HUD::~HUD()
{ {
refreshTimer->stop(); refreshTimer->stop();
imageTimer->stop();
} }
QSize HUD::sizeHint() const QSize HUD::sizeHint() const
...@@ -288,7 +283,7 @@ void HUD::setActiveUAS(UASInterface* uas) ...@@ -288,7 +283,7 @@ void HUD::setActiveUAS(UASInterface* uas)
UAS* u = dynamic_cast<UAS*>(this->uas); UAS* u = dynamic_cast<UAS*>(this->uas);
if (u) { if (u) {
disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64))); disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
disconnect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(requestNewImage())); disconnect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(copyImage()));
} }
} }
...@@ -310,7 +305,7 @@ void HUD::setActiveUAS(UASInterface* uas) ...@@ -310,7 +305,7 @@ void HUD::setActiveUAS(UASInterface* uas)
UAS* u = dynamic_cast<UAS*>(uas); UAS* u = dynamic_cast<UAS*>(uas);
if (u) { if (u) {
connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64))); connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
connect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(requestNewImage())); connect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(copyImage()));
} }
// Set new UAS // Set new UAS
...@@ -675,9 +670,6 @@ void HUD::paintHUD() ...@@ -675,9 +670,6 @@ void HUD::paintHUD()
qDebug() << __FILE__ << __LINE__ << "template image:" << nextOfflineImage; qDebug() << __FILE__ << __LINE__ << "template image:" << nextOfflineImage;
QImage fill = QImage(nextOfflineImage); QImage fill = QImage(nextOfflineImage);
xImageFactor = width() / (float)fill.width();
yImageFactor = height() / (float)fill.height();
glImage = QGLWidget::convertToGLFormat(fill); glImage = QGLWidget::convertToGLFormat(fill);
// Reset to save load efforts // Reset to save load efforts
...@@ -686,9 +678,13 @@ void HUD::paintHUD() ...@@ -686,9 +678,13 @@ void HUD::paintHUD()
glRasterPos2i(0, 0); glRasterPos2i(0, 0);
glPixelZoom(xImageFactor, yImageFactor); xImageFactor = width() / (float)glImage.width();
yImageFactor = height() / (float)glImage.height();
float imageFactor = qMin(xImageFactor, yImageFactor);
glPixelZoom(imageFactor, imageFactor);
// Resize to correct size and fill with image // Resize to correct size and fill with image
glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits()); glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
//qDebug() << "DRAWING GL IMAGE";
} else { } else {
// Blue / brown background // Blue / brown background
paintCenterBackground(roll, pitch, yawTrans); paintCenterBackground(roll, pitch, yawTrans);
...@@ -1633,17 +1629,8 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s ...@@ -1633,17 +1629,8 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
} }
} }
void HUD::requestNewImage() void HUD::copyImage()
{ {
qDebug() << "HUD::requestNewImage()"; qDebug() << "HUD::copyImage()";
// if (!imageRequested) this->glImage = QGLWidget::convertToGLFormat(this->u->getImage());
// {
// this->u->requestImage();
// imageRequested = true;
// }
// else
// {
this->glImage = this->u->getImage();
// imageRequested = false;
// }
} }
src/ui/HUD.h
View file @ 178bc32c
...@@ -88,8 +88,8 @@ public slots: ...@@ -88,8 +88,8 @@ public slots:
void enableHUDInstruments(bool enabled); void enableHUDInstruments(bool enabled);
/** @brief Enable Video */ /** @brief Enable Video */
void enableVideo(bool enabled); void enableVideo(bool enabled);
/** @brief Handler for image transmission */ /** @brief Copy an image from the current active UAS */
void requestNewImage(); void copyImage();
protected slots: protected slots:
...@@ -176,7 +176,6 @@ protected: ...@@ -176,7 +176,6 @@ protected:
int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate
QTimer* refreshTimer; ///< The main timer, controls the update rate QTimer* refreshTimer; ///< The main timer, controls the update rate
QTimer* imageTimer; ///< The timer for the image update rate
QPainter* hudPainter; QPainter* hudPainter;
QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts
QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment