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 @@
#include "SerialLink.h"
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
uasId(id),
startTime(QGC::groundTimeMilliseconds()),
commStatus(COMM_DISCONNECTED),
name(""),
autopilot(-1),
links(new QList<LinkInterface*>()),
unknownPackets(),
mavlink(protocol),
waypointManager(*this),
thrustSum(0),
thrustMax(10),
startVoltage(0),
warnVoltage(9.5f),
warnLevelPercent(20.0f),
currentVoltage(12.0f),
lpVoltage(12.0f),
batteryRemainingEstimateEnabled(false),
mode(-1),
status(-1),
navMode(-1),
onboardTimeOffset(0),
controlRollManual(true),
controlPitchManual(true),
controlYawManual(true),
controlThrustManual(true),
manualRollAngle(0),
manualPitchAngle(0),
manualYawAngle(0),
manualThrust(0),
receiveDropRate(0),
sendDropRate(0),
lowBattAlarm(false),
positionLock(false),
localX(0.0),
localY(0.0),
localZ(0.0),
latitude(0.0),
longitude(0.0),
altitude(0.0),
roll(0.0),
pitch(0.0),
yaw(0.0),
statusTimeout(new QTimer(this)),
paramsOnceRequested(false),
airframe(0),
attitudeKnown(false),
paramManager(NULL)
uasId(id),
startTime(QGC::groundTimeMilliseconds()),
commStatus(COMM_DISCONNECTED),
name(""),
autopilot(-1),
links(new QList<LinkInterface*>()),
unknownPackets(),
mavlink(protocol),
waypointManager(*this),
thrustSum(0),
thrustMax(10),
startVoltage(0),
warnVoltage(9.5f),
warnLevelPercent(20.0f),
currentVoltage(12.0f),
lpVoltage(12.0f),
batteryRemainingEstimateEnabled(false),
mode(-1),
status(-1),
navMode(-1),
onboardTimeOffset(0),
controlRollManual(true),
controlPitchManual(true),
controlYawManual(true),
controlThrustManual(true),
manualRollAngle(0),
manualPitchAngle(0),
manualYawAngle(0),
manualThrust(0),
receiveDropRate(0),
sendDropRate(0),
lowBattAlarm(false),
positionLock(false),
localX(0.0),
localY(0.0),
localZ(0.0),
latitude(0.0),
longitude(0.0),
altitude(0.0),
roll(0.0),
pitch(0.0),
yaw(0.0),
statusTimeout(new QTimer(this)),
paramsOnceRequested(false),
airframe(0),
attitudeKnown(false),
paramManager(NULL)
{
color = UASInterface::getNextColor();
setBattery(LIPOLY, 3);
......@@ -877,6 +877,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
imagePackets = p.packets;
imagePayload = p.payload;
imageQuality = p.jpg_quality;
imageType = p.type;
imageStart = QGC::groundTimeMilliseconds();
}
break;
......@@ -887,6 +888,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
int seq = img.seqnr;
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) {
if (pos <= imageSize) {
imageRecBuffer[pos] = img.data[i];
......@@ -897,15 +906,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
++imagePacketsArrived;
// emit signal if all packets arrived
if ((imagePacketsArrived == imagePackets))
if ((imagePacketsArrived >= imagePackets))
{
// Restart statemachine
imagePacketsArrived = 0;
emit imageReady(this);
qDebug() << "imageReady emitted. all packets arrived";
//this->requestImage();
//qDebug() << "SENDING REQUEST TO GET NEW IMAGE FROM SYSTEM" << uasId;
}
}
break;
......@@ -1120,7 +1126,7 @@ void UAS::startPressureCalibration()
quint64 UAS::getUnixTime(quint64 time)
{
if (time == 0) {
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds();
}
// Check if time is smaller than 40 years,
......@@ -1145,7 +1151,7 @@ quint64 UAS::getUnixTime(quint64 time)
else if (time < 1261440000000000)
#endif
{
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
if (onboardTimeOffset == 0) {
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
}
......@@ -1326,12 +1332,58 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
QImage UAS::getImage()
{
#ifdef MAVLINK_ENABLED_PIXHAWK
image.loadFromData(imageRecBuffer);
#ifdef MAVLINK_ENABLED_PIXHAWK
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
imagePacketsArrived = 0;
//imageRecBuffer.clear();
return image;
#endif
#endif
}
......@@ -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)) {
p.param_id[i] = id.toAscii()[i];
}
// // Null termination at end of string or end of buffer
// else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
// {
// p.param_id[i] = '\0';
// }
// // Null termination at end of string or end of buffer
// else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
// {
// p.param_id[i] = '\0';
// }
// Zero fill
else {
p.param_id[i] = 0;
......
src/uas/UAS.h
View file @ 178bc32c
......@@ -187,7 +187,8 @@ protected: //COMMENTS FOR TEST UNIT
int imagePackets; ///< Number of data packets being sent for this image
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 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
QImage image; ///< Image data of last completely transmitted image
quint64 imageStart;
......
src/ui/HUD.cc
View file @ 178bc32c
......@@ -106,7 +106,6 @@ HUD::HUD(int width, int height, QWidget* parent)
fuelColor(criticalColor),
warningBlinkRate(5),
refreshTimer(new QTimer(this)),
imageTimer(new QTimer(this)),
noCamera(true),
hardwareAcceleration(true),
strongStrokeWidth(1.5f),
......@@ -161,14 +160,11 @@ HUD::HUD(int width, int height, QWidget* parent)
//qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
//fill = QImage(imagePath);
//glImage = QGLWidget::convertToGLFormat(fill);
glImage = QGLWidget::convertToGLFormat(fill);
// Refresh timer
refreshTimer->setInterval(updateInterval);
imageTimer->setInterval(250);
//connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update()));
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
// connect(imageTimer, SIGNAL(timeout()), this, SLOT(requestNewImage())); TODO
// Resize to correct size and fill with image
//glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
......@@ -207,7 +203,6 @@ HUD::HUD(int width, int height, QWidget* parent)
HUD::~HUD()
{
refreshTimer->stop();
imageTimer->stop();
}
QSize HUD::sizeHint() const
......@@ -288,7 +283,7 @@ void HUD::setActiveUAS(UASInterface* uas)
UAS* u = dynamic_cast<UAS*>(this->uas);
if (u) {
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)
UAS* u = dynamic_cast<UAS*>(uas);
if (u) {
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
......@@ -675,9 +670,6 @@ void HUD::paintHUD()
qDebug() << __FILE__ << __LINE__ << "template image:" << nextOfflineImage;
QImage fill = QImage(nextOfflineImage);
xImageFactor = width() / (float)fill.width();
yImageFactor = height() / (float)fill.height();
glImage = QGLWidget::convertToGLFormat(fill);
// Reset to save load efforts
......@@ -686,9 +678,13 @@ void HUD::paintHUD()
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
glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
//qDebug() << "DRAWING GL IMAGE";
} else {
// Blue / brown background
paintCenterBackground(roll, pitch, yawTrans);
......@@ -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()";
// if (!imageRequested)
// {
// this->u->requestImage();
// imageRequested = true;
// }
// else
// {
this->glImage = this->u->getImage();
// imageRequested = false;
// }
qDebug() << "HUD::copyImage()";
this->glImage = QGLWidget::convertToGLFormat(this->u->getImage());
}
src/ui/HUD.h
View file @ 178bc32c
......@@ -88,8 +88,8 @@ public slots:
void enableHUDInstruments(bool enabled);
/** @brief Enable Video */
void enableVideo(bool enabled);
/** @brief Handler for image transmission */
void requestNewImage();
/** @brief Copy an image from the current active UAS */
void copyImage();
protected slots:
......@@ -176,7 +176,6 @@ protected:
int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate
QTimer* refreshTimer; ///< The main timer, controls the update rate
QTimer* imageTimer; ///< The timer for the image update rate
QPainter* hudPainter;
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)
......
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