Improved MAVLink generator widget, added watchdog control data structures (UI work in progress)

qgroundcontrol.pro

@@ -49,7 +49,8 @@ FORMS += src/ui/MainWindow.ui \
     src/ui/HDDisplay.ui \
     src/ui/MAVLinkSettingsWidget.ui \
     src/ui/AudioOutputWidget.ui \
-    src/ui/QGCSensorSettingsWidget.ui
+    src/ui/QGCSensorSettingsWidget.ui \
+    src/ui/watchdog/WatchdogControl.ui
 INCLUDEPATH += src \
     src/ui \
     src/ui/linechart \
@@ -116,7 +117,8 @@ HEADERS += src/MG.h \
     src/uas/SlugsMAV.h \
     src/uas/PxQuadMAV.h \
     src/uas/ArduPilotMAV.h \
-    src/comm/MAVLinkSyntaxHighlighter.h
+    src/comm/MAVLinkSyntaxHighlighter.h \
+    src/ui/watchdog/WatchdogControl.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -166,5 +168,6 @@ SOURCES += src/main.cc \
     src/uas/SlugsMAV.cc \
     src/uas/PxQuadMAV.cc \
     src/uas/ArduPilotMAV.cc \
-    src/comm/MAVLinkSyntaxHighlighter.cc
+    src/comm/MAVLinkSyntaxHighlighter.cc \
+    src/ui/watchdog/WatchdogControl.cc
 RESOURCES = mavground.qrc

src/comm/MAVLinkSimulationLink.cc

@@ -311,7 +311,7 @@ void MAVLinkSimulationLink::mainloop()
 
 
                 // ATTITUDE
-                attitude.msec = time;
+                attitude.usec = time;
                 // Pack message and get size of encoded byte string
                 mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
                 // Allocate buffer with packet data
@@ -398,7 +398,7 @@ void MAVLinkSimulationLink::mainloop()
         typeCounter++;
 
         // Pack message and get size of encoded byte string
-        messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType);
+        messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK);
         // Allocate buffer with packet data
         bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
         //add data into datastream

src/uas/PxQuadMAV.cc

@@ -17,14 +17,36 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
 {
     // Let UAS handle the default message set
     UAS::receiveMessage(link, message);
+    mavlink_message_t* msg = &message;
 
     // Handle your special messages
-    switch (message.msgid)
+    switch (msg->msgid)
     {
-    case MAVLINK_MSG_ID_HEARTBEAT:
+    case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
         {
-            break;
+            mavlink_watchdog_heartbeat_t payload;
+            mavlink_msg_watchdog_heartbeat_decode(msg, &payload);
+            
+            emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count);
         }
+        break;
+        
+    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO:
+        {
+            mavlink_watchdog_process_info_t payload;
+            mavlink_msg_watchdog_process_info_decode(msg, &payload);
+            
+            emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout);
+        }
+        break;
+        
+    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS:
+        {
+            mavlink_watchdog_process_status_t payload;
+            mavlink_msg_watchdog_process_status_decode(msg, &payload);
+            emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
+        }
+        break;
     default:
         // Do nothing
         break;

src/uas/PxQuadMAV.h

@@ -11,6 +11,10 @@ public:
 public slots:
     /** @brief Receive a MAVLink message from this MAV */
     void receiveMessage(LinkInterface* link, mavlink_message_t message);
+signals:
+    void watchdogReceived(int systemId, int watchdogId, int processCount);
+    void processReceived(int systemId, int watchdogId, int processId, QString name, QString arguments, int timeout);
+    void processChanged(int systemId, int watchdogId, int processId, int state, bool muted, int crashed, int pid);
 };
 
 #endif // PXQUADMAV_H

src/uas/UAS.cc

@@ -285,7 +285,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
-                quint64 time = getUnixTime(attitude.msec);
+                quint64 time = getUnixTime(attitude.usec);
                 emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time);
                 emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time);
                 emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time);

src/ui/GaugePanel.cc

-/*=====================================================================
-
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
-
-(c) 2009 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
-
-This file is part of the PIXHAWK project
-
-    PIXHAWK is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    PIXHAWK is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
-
-======================================================================*/
-
-/**
- * @file
- *   @brief Head Down Display / Instrument panel
- *
- *   @author Lorenz Meier <mavteam@student.ethz.ch>
- *
- */
-
-#include "GaugePanel.h"
-
-GaugePanel::GaugePanel(int width, int height, QWidget* parent)
-        : HUD(width, height, parent)
-{
-    vGaugeSpacing = 70.0f;
-    vwidth = 200.0f;
-    vheight = 200.0f;
-}
-
-

src/ui/GaugePanel.h

-/*=====================================================================
-
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
-
-(c) 2009 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
-
-This file is part of the PIXHAWK project
-
-    PIXHAWK is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    PIXHAWK is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
-
-======================================================================*/
-
-/**
- * @file
- *   @brief Head Down Display / Instrument panel
- *
- *   @author Lorenz Meier <mavteam@student.ethz.ch>
- *
- */
-
-#ifndef GAUGEPANEL_H
-#define GAUGEPANEL_H
-
-#include "HUD.h"
-
-class GaugePanel : public HUD
-{
-public:
-    GaugePanel(int width, int height, QWidget* parent);
-};
-
-#endif // GAUGEPANEL_H

src/ui/MainWindow.cc

@@ -95,6 +95,7 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent),
     map->setVisible(false);
     protocol = new XMLCommProtocolWidget(this);
     protocol->setVisible(false);
+    centerStack->addWidget(protocol);
     parameters = new ParameterInterface(this);
     parameters->setVisible(false);
 
@@ -276,6 +277,7 @@ void MainWindow::connectActions()
     connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView()));
     connect(ui.actionSettingsView, SIGNAL(triggered()), this, SLOT(loadSettingsView()));
     connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView()));
+    connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView()));
     connect(ui.actionStyleConfig, SIGNAL(triggered()), this, SLOT(reloadStylesheet()));
 
     // Joystick configuration
@@ -446,10 +448,11 @@ void MainWindow::loadSettingsView()
     linechart->setActive(true);
     centerStack->setCurrentWidget(linechart);
 
+    /*
     // COMM XML
     QDockWidget* container1 = new QDockWidget(tr("MAVLink XML to C Code Generator"), this);
     container1->setWidget(protocol);
-    addDockWidget(Qt::LeftDockWidgetArea, container1);
+    addDockWidget(Qt::LeftDockWidgetArea, container1);*/
 
     // ONBOARD PARAMETERS
     QDockWidget* container6 = new QDockWidget(tr("Onboard Parameters"), this);
@@ -496,6 +499,12 @@ void MainWindow::loadEngineerView()
     this->show();
 }
 
+void MainWindow::loadMAVLinkView()
+{
+    clearView();
+    centerStack->setCurrentWidget(protocol);
+}
+
 void MainWindow::loadAllView()
 {
     clearView();

src/ui/MainWindow.h

@@ -113,6 +113,8 @@ public slots:
     void loadSettingsView();
     /** @brief Load view with all widgets */
     void loadAllView();
+    /** @brief Load MAVLink XML generator view */
+    void loadMAVLinkView();
 
     /** @brief Reload the CSS style sheet */
     void reloadStylesheet();

src/ui/MainWindow.ui

@@ -35,7 +35,7 @@
      <x>0</x>
      <y>0</y>
      <width>1000</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuMGround">
@@ -74,6 +74,7 @@
     <addaction name="actionOperatorView"/>
     <addaction name="actionSettingsView"/>
     <addaction name="separator"/>
+    <addaction name="actionShow_MAVLink_view"/>
     <addaction name="actionShow_full_view"/>
     <addaction name="actionStyleConfig"/>
    </widget>
@@ -253,6 +254,15 @@
     <string>Show full view</string>
    </property>
   </action>
+  <action name="actionShow_MAVLink_view">
+   <property name="icon">
+    <iconset resource="../../mavground.qrc">
+     <normaloff>:/images/devices/network-wireless.svg</normaloff>:/images/devices/network-wireless.svg</iconset>
+   </property>
+   <property name="text">
+    <string>Show MAVLink view</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/ui/watchdog/WatchdogControl.cc

+#include "WatchdogControl.h"
+#include "ui_WatchdogControl.h"
+
+#include <QDebug>
+
+WatchdogControl::WatchdogControl(QWidget *parent) :
+        QWidget(parent),
+        ui(new Ui::WatchdogControl)
+{
+    ui->setupUi(this);
+}
+
+WatchdogControl::~WatchdogControl()
+{
+    delete ui;
+}
+
+void WatchdogControl::updateWatchdog(int systemId, int watchdogId, unsigned int processCount)
+{
+    // request the watchdog with the given ID
+    WatchdogInfo& watchdog = this->getWatchdog(systemId, watchdogId);
+
+    // if the proces count doesn't match, the watchdog is either new or has changed - create a new vector with new (and empty) ProcessInfo structs.
+    if (watchdog.processes_.size() != processCount)
+        watchdog.processes_ = std::vector<ProcessInfo>(processCount);
+
+    // start the timeout timer
+    //watchdog.timeoutTimer_.reset();
+    //qDebug() << "<-- received mavlink_watchdog_heartbeat_t " << msg->sysid << " / " << payload.watchdog_id << " / " << payload.process_count << std::endl;
+}
+
+void WatchdogControl::addProcess(int systemId, int watchdogId, int processId, QString name, QString arguments, int timeout)
+{
+    // request the watchdog and the process with the given IDs
+    WatchdogInfo& watchdog = this->getWatchdog(systemId, watchdogId);
+    ProcessInfo& process = watchdog.getProcess(processId);
+
+    // store the process information in the ProcessInfo struct
+    process.name_ = name.toStdString();
+    process.arguments_ = arguments.toStdString();
+    process.timeout_ = timeout;
+    //qDebug() << "<-- received mavlink_watchdog_process_info_t " << msg->sysid << " / " << (const char*)payload.name << " / " << (const char*)payload.arguments << " / " << payload.timeout << std::endl;
+}
+
+
+void WatchdogControl::updateProcess(int systemId, int watchdogId, int processId, int state, bool muted, int crashes, int pid)
+{
+    // request the watchdog and the process with the given IDs
+    WatchdogInfo& watchdog = this->getWatchdog(systemId, watchdogId);
+    ProcessInfo& process = watchdog.getProcess(processId);
+
+    // store the status information in the ProcessInfo struct
+    process.state_ = static_cast<ProcessInfo::State::Enum>(state);
+    process.muted_ = muted;
+    process.crashes_ = crashes;
+    process.pid_ = pid;
+
+    //process.updateTimer_.reset();
+    //qDebug() << "<-- received mavlink_watchdog_process_status_t " << msg->sysid << " / " << payload.state << " / " << payload.muted << " / " << payload.crashes << " / " << payload.pid << std::endl;
+}
+
+/**
+    @brief Returns a WatchdogInfo struct that belongs to the watchdog with the given system-ID and watchdog-ID
+*/
+WatchdogControl::WatchdogInfo& WatchdogControl::getWatchdog(uint8_t systemId, uint16_t watchdogId)
+{
+    WatchdogID id(systemId, watchdogId);
+
+    std::map<WatchdogID, WatchdogInfo>::iterator it = this->watchdogs_.find(id);
+    if (it != this->watchdogs_.end())
+    {
+        // the WatchdogInfo struct already exists in the map, return it
+        return it->second;
+    }
+    else
+    {
+        // the WatchdogInfo struct doesn't exist - request info and status for all processes and create the struct
+        this->sendCommand(id, WatchdogControl::ALL, Command::RequestInfo);
+        this->sendCommand(id, WatchdogControl::ALL, Command::RequestStatus);
+        return this->watchdogs_[id];
+    }
+}
+
+/**
+    @brief Returns a ProcessInfo struct that belongs to the process with the given ID.
+*/
+WatchdogControl::ProcessInfo& WatchdogControl::WatchdogInfo::getProcess(uint16_t index)
+{
+    // if the index is out of bounds, resize the vector
+    if (index >= this->processes_.size())
+        this->processes_.resize(index + 1);
+
+    return this->processes_[index];
+}
+
+/**
+    @brief Sends a watchdog command to a process on a given watchdog.
+    @param w_id The WatchdogID struct (containing system-ID and watchdog-ID) that identifies the watchdog
+    @param p_id The process-ID
+    @param command The command-ID
+*/
+void WatchdogControl::sendCommand(const WatchdogID& w_id, uint16_t p_id, Command::Enum command)
+{
+    /*
+    mavlink_watchdog_command_t payload;
+    payload.target_system_id = w_id.system_id_;
+    payload.watchdog_id = w_id.watchdog_id_;
+    payload.process_id = p_id;
+    payload.command_id = (uint8_t)command;
+
+    mavlink_message_t msg;
+    mavlink_msg_watchdog_command_encode(sysid, compid, &msg, &payload);
+    mavlink_message_t_publish(this->lcm_, "MAVLINK", &msg);*/
+//std::cout << "--> sent mavlink_watchdog_command_t " << payload.target_system_id << " / " << payload.watchdog_id << " / " << payload.process_id << " / " << (int)payload.command_id << std::endl;
+}
+
+void WatchdogControl::changeEvent(QEvent *e)
+{
+    QWidget::changeEvent(e);
+    switch (e->type()) {
+    case QEvent::LanguageChange:
+        ui->retranslateUi(this);
+        break;
+    default:
+        break;
+    }
+}

src/ui/watchdog/WatchdogControl.h

+#ifndef WATCHDOGCONTROL_H
+#define WATCHDOGCONTROL_H
+
+#include <QWidget>
+#include <QTimer>
+
+#include <map>
+#include <string>
+#include <sstream>
+
+namespace Ui {
+    class WatchdogControl;
+}
+
+class WatchdogControl : public QWidget {
+    Q_OBJECT
+public:
+
+
+    ///! Command codes, used to send and receive commands over lcm
+    struct Command
+    {
+        enum Enum
+        {
+            Start         = 0,
+            Restart       = 1,
+            Stop          = 2,
+            Mute          = 3,
+            Unmute        = 4,
+
+            RequestInfo   = 254,
+            RequestStatus = 255
+        };
+    };
+
+    ///! This struct represents a process on the watchdog. Used to store all values.
+    struct ProcessInfo
+    {
+        ///! Process state - each process is in exactly one of those states (except unknown, that's just to initialize it)
+        struct State
+        {
+            enum Enum
+            {
+                Unknown       = 0,
+                Running       = 1,
+                Stopped       = 2,
+                Stopped_OK    = 3,
+                Stopped_ERROR = 4
+            };
+        };
+
+        ///! Constructor - initialize the values
+        ProcessInfo() : timeout_(0), state_(State::Unknown), muted_(false), crashes_(0), pid_(-1) {}
+
+        std::string name_;      ///< The name of the process
+        std::string arguments_; ///< The arguments (argv of the process)
+
+        int32_t timeout_;       ///< Heartbeat timeout value (in microseconds)
+
+        State::Enum state_;     ///< The current state of the process
+        bool muted_;            ///< True if the process is currently muted
+        uint16_t crashes_;      ///< The number of crashes
+        int32_t pid_;           ///< The PID of the process
+
+    //    Timer requestTimer_;    ///< Internal timer, used to repeat status and info requests after some time (in case of packet loss)
+    //    Timer updateTimer_;     ///< Internal timer, used to measure the time since the last update (used only for graphics)
+    };
+
+    ///! This struct identifies a watchdog. It's a combination of system-ID and watchdog-ID. implements operator< to be used as key in a std::map
+    struct WatchdogID
+    {
+        ///! Constructor - initialize the values
+        WatchdogID(uint8_t system_id, uint16_t watchdog_id) : system_id_(system_id), watchdog_id_(watchdog_id) {}
+
+        uint8_t system_id_;     ///< The system-ID
+        uint16_t watchdog_id_;  ///< The watchdog-ID
+
+        ///! Comparison operator which is used by std::map
+        inline bool operator<(const WatchdogID& other) const
+            { return (this->system_id_ != other.system_id_) ? (this->system_id_ < other.system_id_) : (this->watchdog_id_ < other.watchdog_id_); }
+
+    };
+
+    ///! This struct represents a watchdog
+    struct WatchdogInfo
+    {
+        ProcessInfo& getProcess(uint16_t index);
+
+        std::vector<ProcessInfo> processes_;    ///< A vector containing all processes running on this watchdog
+        QTimer* timeoutTimer_;                    ///< Internal timer, used to measure the time since the last heartbeat message
+    };
+
+    WatchdogControl(QWidget *parent = 0);
+    ~WatchdogControl();
+
+    static const uint16_t ALL         = (uint16_t)-1;   ///< A magic value for a process-ID which addresses "all processes"
+    static const uint16_t ALL_RUNNING = (uint16_t)-2;   ///< A magic value for a process-ID which addresses "all running processes"
+    static const uint16_t ALL_CRASHED = (uint16_t)-3;   ///< A magic value for a process-ID which addresses "all crashed processes"
+
+public slots:
+    void updateWatchdog(int systemId, int watchdogId, unsigned int processCount);
+    void addProcess(int systemId, int watchdogId, int processId, QString name, QString arguments, int timeout);
+    void updateProcess(int systemId, int watchdogId, int processId, int state, bool muted, int crashed, int pid);
+
+protected:
+    void changeEvent(QEvent *e);
+
+private:
+    Ui::WatchdogControl *ui;
+
+    void sendCommand(const WatchdogID& w_id, uint16_t p_id, Command::Enum command);
+
+    WatchdogInfo& getWatchdog(uint8_t system_id, uint16_t watchdog_id);
+
+    std::map<WatchdogID, WatchdogInfo> watchdogs_;          ///< A map containing all watchdogs which are currently active
+    QTimer updateTimer_;
+};
+
+#endif // WATCHDOGCONTROL_H
+
+///! Convert a value to std::string
+template <class T>
+std::string convertToString(T value)
+{
+std::ostringstream oss;
+oss << value;
+return oss.str();
+}

src/ui/watchdog/WatchdogControl.ui

+<ui version="4.0">
+ <author/>
+ <comment/>
+ <exportmacro/>
+ <class>WatchdogControl</class>
+ <widget class="QWidget" name="WatchdogControl">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+ </widget>
+ <pixmapfunction/>
+ <connections/>
+</ui>