From fc38a557781aa9745008f09ecc945da93199fc02 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 24 May 2010 18:49:01 +0200 Subject: [PATCH] Improved MAVLink generator widget, added watchdog control data structures (UI work in progress) --- qgroundcontrol.pro | 9 +- src/comm/MAVLinkSimulationLink.cc | 4 +- src/uas/PxQuadMAV.cc | 28 ++++++- src/uas/PxQuadMAV.h | 4 + src/uas/UAS.cc | 2 +- src/ui/GaugePanel.cc | 42 ---------- src/ui/GaugePanel.h | 43 ---------- src/ui/MainWindow.cc | 11 ++- src/ui/MainWindow.h | 2 + src/ui/MainWindow.ui | 12 ++- src/ui/watchdog/WatchdogControl.cc | 127 ++++++++++++++++++++++++++++ src/ui/watchdog/WatchdogControl.h | 128 +++++++++++++++++++++++++++++ src/ui/watchdog/WatchdogControl.ui | 21 +++++ 13 files changed, 337 insertions(+), 96 deletions(-) delete mode 100644 src/ui/GaugePanel.cc delete mode 100644 src/ui/GaugePanel.h create mode 100644 src/ui/watchdog/WatchdogControl.cc create mode 100644 src/ui/watchdog/WatchdogControl.h create mode 100644 src/ui/watchdog/WatchdogControl.ui diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 477d0cdfb..9e633857b 100644 --- a/qgroundcontrol.pro +++ b/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 diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 1111b7e71..d38966668 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/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 diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 1a6aeae8a..28db2d077 100644 --- a/src/uas/PxQuadMAV.cc +++ b/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; diff --git a/src/uas/PxQuadMAV.h b/src/uas/PxQuadMAV.h index 05f7688fd..ba58a1168 100644 --- a/src/uas/PxQuadMAV.h +++ b/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 diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c01e62c8b..2c0a28770 100644 --- a/src/uas/UAS.cc +++ b/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); diff --git a/src/ui/GaugePanel.cc b/src/ui/GaugePanel.cc deleted file mode 100644 index dc87184ae..000000000 --- a/src/ui/GaugePanel.cc +++ /dev/null @@ -1,42 +0,0 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009 PIXHAWK PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Head Down Display / Instrument panel - * - * @author Lorenz Meier - * - */ - -#include "GaugePanel.h" - -GaugePanel::GaugePanel(int width, int height, QWidget* parent) - : HUD(width, height, parent) -{ - vGaugeSpacing = 70.0f; - vwidth = 200.0f; - vheight = 200.0f; -} - - diff --git a/src/ui/GaugePanel.h b/src/ui/GaugePanel.h deleted file mode 100644 index ea403ec9f..000000000 --- a/src/ui/GaugePanel.h +++ /dev/null @@ -1,43 +0,0 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009 PIXHAWK PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Head Down Display / Instrument panel - * - * @author Lorenz Meier - * - */ - -#ifndef GAUGEPANEL_H -#define GAUGEPANEL_H - -#include "HUD.h" - -class GaugePanel : public HUD -{ -public: - GaugePanel(int width, int height, QWidget* parent); -}; - -#endif // GAUGEPANEL_H diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index dfedb4fb1..e07e037b6 100644 --- a/src/ui/MainWindow.cc +++ b/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(); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 9cb2f23a7..a5fbfb939 100644 --- a/src/ui/MainWindow.h +++ b/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(); diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index d77e4f297..4e0fd43e4 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -35,7 +35,7 @@ 0 0 1000 - 25 + 22 @@ -74,6 +74,7 @@ + @@ -253,6 +254,15 @@ Show full view + + + + :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg + + + Show MAVLink view + + diff --git a/src/ui/watchdog/WatchdogControl.cc b/src/ui/watchdog/WatchdogControl.cc new file mode 100644 index 000000000..6e0c0a272 --- /dev/null +++ b/src/ui/watchdog/WatchdogControl.cc @@ -0,0 +1,127 @@ +#include "WatchdogControl.h" +#include "ui_WatchdogControl.h" + +#include + +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(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(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::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; + } +} diff --git a/src/ui/watchdog/WatchdogControl.h b/src/ui/watchdog/WatchdogControl.h new file mode 100644 index 000000000..486dff865 --- /dev/null +++ b/src/ui/watchdog/WatchdogControl.h @@ -0,0 +1,128 @@ +#ifndef WATCHDOGCONTROL_H +#define WATCHDOGCONTROL_H + +#include +#include + +#include +#include +#include + +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 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 watchdogs_; ///< A map containing all watchdogs which are currently active + QTimer updateTimer_; +}; + +#endif // WATCHDOGCONTROL_H + +///! Convert a value to std::string +template +std::string convertToString(T value) +{ +std::ostringstream oss; +oss << value; +return oss.str(); +} diff --git a/src/ui/watchdog/WatchdogControl.ui b/src/ui/watchdog/WatchdogControl.ui new file mode 100644 index 000000000..a05f60309 --- /dev/null +++ b/src/ui/watchdog/WatchdogControl.ui @@ -0,0 +1,21 @@ + + + + + WatchdogControl + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + -- 2.22.0