Partly fixed map issues, updated joystick interface

images/style-mission.css

@@ -251,6 +251,21 @@ QSlider::groove:horizontal {
      margin: -5px 0; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */
      border-radius: 3px;
  }
+ 
+ QSlider::groove:vertical {
+     border: 1px solid #999999;
+     width: 4px; /* the groove expands to the size of the slider by default. by giving it a height, it has a fixed size */
+     background: qlineargradient(x1:0, y1:0, x2:1, y2:0, stop:0 #4A4A4F, stop:1 #4A4A4F);
+     margin: 2px 0;
+ }
+
+ QSlider::handle:vertical {
+     background-color: qlineargradient(x1: 0, y1: 0, x2: 1, y2: 0, stop: 0 #232228, stop: 1 #020208);
+     border: 2px solid #379AC3;
+     height: 18px;
+     margin: 0 -5px; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */
+     border-radius: 3px;
+ }
 
 QPushButton#forceLandButton {
 	font-weight: bold;

lib/QMapControl/src/mapcontrol.cpp

@@ -377,7 +377,7 @@ namespace qmapcontrol
     {
         layermanager->setZoom(zoomlevel);
 
-        qDebug() << "MAPCONTROL: Set zoomlevel to:" << zoomlevel << "at " << __FILE__ << __LINE__;
+        //qDebug() << "MAPCONTROL: Set zoomlevel to:" << zoomlevel << "at " << __FILE__ << __LINE__;
 
         update();
     }

lib/QMapControl/src/mapnetwork.cpp

@@ -93,13 +93,19 @@ namespace qmapcontrol
                         // qDebug() << "Network loaded: " << (loaded);
                         parent->receivedImage(pm, url);
                     }
+                    else if (pm.width() == 0 || pm.height() == 0)
+                    {
+                        // Silently ignore map request for a
+                        // 0xn pixel map
+
+                    }
                     else
                     {
                         // QGC FIXME Error is currently undetected
                         // TODO Error is currently undetected
                         //qDebug() << "NETWORK_PIXMAP_ERROR: " << ax;
                         qDebug() << "QMapControl external library: ERROR loading map:" << "width:" << pm.width() << "heigh:" << pm.height() << "at " << __FILE__ << __LINE__;
-                        qDebug() << "HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__;
+                        //qDebug() << "HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__;
                     }
                 }
 

src/comm/MAVLinkSimulationLink.cc

@@ -159,8 +159,6 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
         readyBuffer.enqueue(*(buf + i));
     }
 //    readyBufferMutex.unlock();
-
-    qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
 }
 
 void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)

src/comm/MAVLinkSimulationMAV.cc

@@ -48,9 +48,15 @@ void MAVLinkSimulationMAV::mainloop()
 
     if (!firstWP)
     {
-        x += (nextSPX - previousSPX) * 0.01;
-        y += (nextSPY - previousSPY) * 0.01;
-        z += (nextSPZ - previousSPZ) * 0.1;
+        double xm = (nextSPX - x) * 0.01;
+        double ym = (nextSPY - y) * 0.01;
+        double zm = (nextSPZ - z) * 0.1;
+
+        x += xm;
+        y += ym;
+        z += zm;
+
+        //if (xm < 0.001) xm
     }
     else
     {

src/comm/SerialLink.cc

@@ -180,13 +180,13 @@ void SerialLink::writeBytes(const char* data, qint64 size)
         // Increase write counter
         bitsSentTotal += size * 8;
 
-        //        int i;
-        //        for (i=0; i<size; i++)
-        //        {
-        //            unsigned char v=data[i];
-
-        //            //fprintf(stderr,"%02x ", v);
-        //        }
+        int i;
+        for (i=0; i<size; i++)
+        {
+            unsigned char v =data[i];
+            qDebug("%02x ", v);
+        }
+        qDebug("\n");
     }
 }
 

src/comm/UDPLink.cc

@@ -93,30 +93,16 @@ void UDPLink::writeBytes(const char* data, qint64 size)
     {
         QHostAddress currentHost = hosts->at(h);
         quint16 currentPort = ports->at(h);
-        //        QList<quint16> currentPorts = ports->values(currentHost);
-        //        for (int p = 0; p < currentPorts.size(); p++)
-        //        {
-        //            quint16 currentPort = currentPorts.at(p);
-        //qDebug() << "Sent message to " << currentHost << ":" << currentPort << "at" << __FILE__ << ":" << __LINE__;
-        socket->writeDatagram(data, size, currentHost, currentPort);
-        //        }
-    }
-
 
-    //if(socket->write(data, size) > 0) {
+        for (int i=0; i<size; i++)
+        {
+            unsigned char v =data[i];
+            qDebug("%02x ", v);
+        }
+        qDebug("\n");
 
-//    qDebug() << "Transmitted " << size << "bytes:";
-//
-//    /* Increase write counter */
-//    bitsSentTotal += size * 8;
-//
-//    int i;
-//    for (i=0; i<size; i++){
-//        unsigned int v=data[i];
-//
-//        fprintf(stderr,"%02x ", v);
-//    }
-    //}
+        socket->writeDatagram(data, size, currentHost, currentPort);
+    }
 }
 
 /**

src/input/JoystickInput.cc

@@ -52,7 +52,8 @@ JoystickInput::JoystickInput() :
         thrustAxis(3),
         xAxis(1),
         yAxis(0),
-        yawAxis(2)
+        yawAxis(2),
+        joystickName(tr("Unitinialized"))
 {
     for (int i = 0; i < 10; i++)
     {
@@ -63,7 +64,7 @@ JoystickInput::JoystickInput() :
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 
     // Enter main loop
-    start();
+    //start();
 }
 
 void JoystickInput::setActiveUAS(UASInterface* uas)
@@ -88,7 +89,10 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
         connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double)));
         connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int)));
     }
-
+    if (!isRunning())
+    {
+        start();
+    }
 }
 
 void JoystickInput::init()
@@ -118,6 +122,7 @@ void JoystickInput::init()
     for(int i=0; i < SDL_NumJoysticks(); i++ )
     {
         printf("\t- %s\n", SDL_JoystickName(i));
+        joystickName = QString(SDL_JoystickName(i));
     }
 
     printf("\nOpened %s\n", SDL_JoystickName(defaultIndex));
@@ -125,6 +130,9 @@ void JoystickInput::init()
     SDL_JoystickEventState(SDL_ENABLE);
 
     joystick = SDL_JoystickOpen(defaultIndex);
+
+    // Make sure active UAS is set
+    setActiveUAS(UASManager::instance()->getActiveUAS());
 }
 
 /**
@@ -282,3 +290,8 @@ void JoystickInput::run()
     }
 
 }
+
+const QString& JoystickInput::getName()
+{
+    return joystickName;
+}

src/input/JoystickInput.h

@@ -50,6 +50,8 @@ public:
     JoystickInput();
     void run();
 
+    const QString& getName();
+
     const double sdlJoystickMin;
     const double sdlJoystickMax;
 
@@ -68,6 +70,7 @@ protected:
     int yAxis;
     int yawAxis;
     SDL_Event event;
+    QString joystickName;
 
     void init();
 

src/uas/UAS.cc

@@ -934,6 +934,7 @@ void UAS::setMode(int mode)
 {
     if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING)
     {
+        this->mode = mode;
         mavlink_message_t msg;
         mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);
         sendMessage(msg);
@@ -1432,15 +1433,19 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     manualYawAngle = yaw * yawScaling;
     manualThrust = thrust * thrustScaling;
 
-    //    if(mode == (int)MAV_MODE_MANUAL)
-    //    {
-    mavlink_message_t message;
-    mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
-    sendMessage(message);
-    qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
+    if(mode == (int)MAV_MODE_MANUAL)
+    {
+        mavlink_message_t message;
+        mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
+        sendMessage(message);
+        qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
 
-    emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
-    //    }
+        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
+    }
+    else
+    {
+        qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
+    }
 }
 
 int UAS::getSystemType()

src/ui/JoystickWidget.cc

@@ -9,12 +9,13 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
     m_ui->setupUi(this);
     this->joystick = joystick;
 
-
     connect(this->joystick, SIGNAL(joystickChanged(double,double,double,double,int,int)), this, SLOT(updateJoystick(double,double,double,double,int,int)));
     connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(pressKey(int)));
 
     // Display the widget
     this->window()->setWindowTitle(tr("Joystick Settings"));
+    if (joystick) updateStatus(tr("Found joystick: %1").arg(joystick->getName()));
+
     this->show();
 }
 
@@ -69,15 +70,71 @@ void JoystickWidget::setZ(float z)
 void JoystickWidget::setHat(float x, float y)
 {
     qDebug() << __FILE__ << __LINE__ << "HAT X:" << x << "HAT Y:" << y;
+    updateStatus(tr("Hat position: x: %1, y: %2").arg(x, y));
+}
+
+void JoystickWidget::clearKeys()
+{
+    QString colorstyle;
+    QColor buttonStyleColor = QColor(200, 20, 20);
+    colorstyle = QString("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: %1;}").arg(buttonStyleColor.name());
+
+    m_ui->buttonLabel0->setStyleSheet(colorstyle);
+    m_ui->buttonLabel1->setStyleSheet(colorstyle);
+    m_ui->buttonLabel2->setStyleSheet(colorstyle);
+    m_ui->buttonLabel3->setStyleSheet(colorstyle);
+    m_ui->buttonLabel4->setStyleSheet(colorstyle);
+    m_ui->buttonLabel5->setStyleSheet(colorstyle);
+    m_ui->buttonLabel6->setStyleSheet(colorstyle);
+    m_ui->buttonLabel7->setStyleSheet(colorstyle);
+    m_ui->buttonLabel8->setStyleSheet(colorstyle);
+    m_ui->buttonLabel9->setStyleSheet(colorstyle);
 }
 
 void JoystickWidget::pressKey(int key)
 {
     QString colorstyle;
-    QColor heartbeatColor = QColor(20, 200, 20);
-    colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}",
-                                    heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue());
-    m_ui->button0Label->setStyleSheet(colorstyle);
-    m_ui->button0Label->setAutoFillBackground(true);
+    QColor buttonStyleColor = QColor(20, 200, 20);
+    colorstyle = QString("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: %1;}").arg(buttonStyleColor.name());
+    switch(key)
+    {
+    case 0:
+        m_ui->buttonLabel0->setStyleSheet(colorstyle);
+        break;
+    case 1:
+        m_ui->buttonLabel1->setStyleSheet(colorstyle);
+        break;
+    case 2:
+        m_ui->buttonLabel2->setStyleSheet(colorstyle);
+        break;
+    case 3:
+        m_ui->buttonLabel3->setStyleSheet(colorstyle);
+        break;
+    case 4:
+        m_ui->buttonLabel4->setStyleSheet(colorstyle);
+        break;
+    case 5:
+        m_ui->buttonLabel5->setStyleSheet(colorstyle);
+        break;
+    case 6:
+        m_ui->buttonLabel6->setStyleSheet(colorstyle);
+        break;
+    case 7:
+        m_ui->buttonLabel7->setStyleSheet(colorstyle);
+        break;
+    case 8:
+        m_ui->buttonLabel8->setStyleSheet(colorstyle);
+        break;
+    case 9:
+        m_ui->buttonLabel9->setStyleSheet(colorstyle);
+        break;
+    }
+    QTimer::singleShot(20, this, SLOT(clearKeys()));
     qDebug() << __FILE__ << __LINE__ << "KEY" << key << " pressed on joystick";
+    updateStatus(tr("Key %1 pressed").arg(key));
+}
+
+void JoystickWidget::updateStatus(const QString& status)
+{
+
 }

src/ui/JoystickWidget.h

@@ -67,8 +67,12 @@ public:
     void setZ(float z);
     /** @brief Hat switch position */
     void setHat(float x, float y);
+    /** @brief Clear keys */
+    void clearKeys();
     /** @brief Joystick keys, as labeled on the joystick */
     void pressKey(int key);
+    /** @brief Update status string */
+    void updateStatus(const QString& status);
 
 protected:
     /** @brief UI change event */

src/ui/MainWindow.cc

@@ -202,6 +202,12 @@ MainWindow::MainWindow(QWidget *parent):
 
     connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*)));
 
+    // Connect user interface devices
+    if (!joystick)
+    {
+        joystick = new JoystickInput();
+    }
+
     // Enable and update view
     presentView();
 }
@@ -492,11 +498,6 @@ void MainWindow::buildPxWidgets()
 
     // Dialogue widgets
     //FIXME: free memory in destructor
-    if (!joystick)
-    {
-        joystick    = new JoystickInput();
-    }
-
 }
 
 void MainWindow::buildSlugsWidgets()
@@ -1159,14 +1160,20 @@ void MainWindow::connectCommonActions()
     // Audio output
     ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted());
     connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool)));
+
+    // User interaction
+    // NOTE: Joystick thread is not started and
+    // configuration widget is not instantiated
+    // unless it is actually used
+    // so no ressources spend on this.
+    ui.actionJoystickSettings->setVisible(true);
+    // Joystick configuration
+    connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure()));
 }
 
 void MainWindow::connectPxActions()
 {
-    ui.actionJoystickSettings->setVisible(true);
 
-    // Joystick configuration
-    connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure()));
 
 }
 
@@ -1219,7 +1226,15 @@ void MainWindow::showRoadMap()
 
 void MainWindow::configure()
 {
-    joystickWidget = new JoystickWidget(joystick, this);
+    if (!joystickWidget)
+    {
+        if (!joystick->isRunning())
+        {
+            joystick->start();
+        }
+        joystickWidget = new JoystickWidget(joystick);
+    }
+    joystickWidget->show();
 }
 
 void MainWindow::addLink()

src/ui/MainWindow.ui

@@ -51,7 +51,7 @@
      <x>0</x>
      <y>0</y>
      <width>800</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuMGround">
@@ -62,6 +62,7 @@
     <addaction name="actionSimulate"/>
     <addaction name="separator"/>
     <addaction name="actionMuteAudioOutput"/>
+    <addaction name="actionJoystickSettings"/>
     <addaction name="actionPreferences"/>
     <addaction name="actionReloadStyle"/>
     <addaction name="separator"/>
@@ -215,7 +216,7 @@
      <normaloff>:/images/devices/input-gaming.svg</normaloff>:/images/devices/input-gaming.svg</iconset>
    </property>
    <property name="text">
-    <string>Joystick settings</string>
+    <string>Joystick Test</string>
    </property>
    <property name="visible">
     <bool>true</bool>

src/ui/MapWidget.cc

@@ -469,7 +469,7 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
         qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();
 
         // Refresh the screen
-        mc->updateRequest(tempPoint->boundingBox().toRect());
+        mc->updateRequestNew();//(tempPoint->boundingBox().toRect());
     }
 
     ////    // emit signal mouse was clicked
@@ -630,6 +630,8 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, ""));
     }
 
+    mc->updateRequest(p->boundingBox().toRect());
+
     //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 
 
@@ -745,7 +747,7 @@ void MapWidget::clearWaypoints()
     waypointPath->setPoints(wps);
     mc->layer("Waypoints")->addGeometry(waypointPath);
     wpIndex.clear();
-    mc->updateRequest(waypointPath->boundingBox().toRect());
+    mc->updateRequestNew();//(waypointPath->boundingBox().toRect());
 
     if(createPath->isChecked())
     {
@@ -764,7 +766,7 @@ void MapWidget::clearPath()
         mc->layer("Tracking")->addGeometry(lsNew);
     }
     // FIXME update this with update request only for bounding box of trails
-    mc->updateRequest(QRect(0, 0, width(), height()));
+    mc->updateRequestNew();//(QRect(0, 0, width(), height()));
 }
 
 void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon)
@@ -785,7 +787,7 @@ void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, floa
         point2Find->setCoordinate(coordinate);
 
         // Refresh the screen
-        mc->updateRequest(point2Find->boundingBox().toRect());
+        mc->updateRequestNew();//(point2Find->boundingBox().toRect());
     }
 
 

src/ui/map/MAV2DIcon.cc

@@ -40,53 +40,56 @@ void MAV2DIcon::setPen(QPen* pen)
 void MAV2DIcon::setYaw(float yaw)
 {
     this->yaw = yaw;
+    drawIcon(mypen);
 }
 
 void MAV2DIcon::drawIcon(QPen* pen)
 {
-    if (pen)
-    {
-        mypixmap = new QPixmap(radius+1, radius+1);
-        //mypixmap->fill(Qt::transparent);
-        QPainter painter(mypixmap);
 
-        // DRAW MICRO AIR VEHICLE
-        QPointF p(radius/2, radius/2);
+    mypixmap = new QPixmap(radius+1, radius+1);
+    mypixmap->fill(Qt::transparent);
+    QPainter painter(mypixmap);
 
-        float waypointSize = radius;
-        QPolygonF poly(3);
-        // Top point
-        poly.replace(0, QPointF(p.x(), p.y()+waypointSize/2.0f));
-        // Right point
-        poly.replace(1, QPointF(p.x()-waypointSize/2.0f, p.y()-waypointSize/2.0f));
-        // Left point
-        poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f));
+    // DRAW WAYPOINT
+    QPointF p(radius/2, radius/2);
 
-        //    // Select color based on if this is the current waypoint
-        //    if (list.at(i)->getCurrent())
-        //    {
-        //        color = QGC::colorCyan;//uas->getColor();
-        //        pen.setWidthF(refLineWidthToPen(0.8f));
-        //    }
-        //    else
-        //    {
-        //        color = uas->getColor();
-        //        pen.setWidthF(refLineWidthToPen(0.4f));
-        //    }
+    float waypointSize = radius;
+    QPolygonF poly(4);
+    // Top point
+    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+    // Right point
+    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+    // Bottom point
+    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
 
-        //pen.setColor(color);
-        if (pen)
-        {
-            pen->setWidthF(2);
-            painter.setPen(*pen);
-        }
-        else
-        {
-            QPen pen2(Qt::yellow);
-            pen2.setWidth(2);
-            painter.setPen(pen2);
-        }
-        painter.setBrush(Qt::NoBrush);
-        painter.drawPolygon(poly);
+//    // Select color based on if this is the current waypoint
+//    if (list.at(i)->getCurrent())
+//    {
+//        color = QGC::colorCyan;//uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.8f));
+//    }
+//    else
+//    {
+//        color = uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.4f));
+//    }
+
+    //pen.setColor(color);
+    if (pen)
+    {
+        pen->setWidthF(2);
+        painter.setPen(*pen);
+    }
+    else
+    {
+        QPen pen2(Qt::red);
+        pen2.setWidth(2);
+        painter.setPen(pen2);
     }
+    painter.setBrush(Qt::NoBrush);
+
+    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+    painter.drawPolygon(poly);
 }

src/ui/map/Waypoint2DIcon.cc

@@ -35,6 +35,7 @@ void Waypoint2DIcon::setPen(QPen* pen)
 void Waypoint2DIcon::setYaw(float yaw)
 {
     this->yaw = yaw;
+    drawIcon(mypen);
 }
 
 void Waypoint2DIcon::drawIcon(QPen* pen)