diff --git a/.gitignore b/.gitignore index 15d9c7e..2540409 100644 --- a/.gitignore +++ b/.gitignore @@ -16,6 +16,7 @@ tmp debug release qgroundcontrol +mavlinkgen *.wav qgroundcontrol.xcodeproj/** doc/html diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 28db2d0..6deb02f 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -52,3 +52,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) break; } } + +void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command) +{ + mavlink_watchdog_command_t payload; + payload.target_system_id = uasId; + payload.watchdog_id = watchdogId; + payload.process_id = processId; + payload.command_id = (uint8_t)command; + + mavlink_message_t msg; + mavlink_msg_watchdog_command_encode(sysid, compid, &msg, &payload); + sendMessage(msg); +} diff --git a/src/uas/PxQuadMAV.h b/src/uas/PxQuadMAV.h index ba58a11..af12ab2 100644 --- a/src/uas/PxQuadMAV.h +++ b/src/uas/PxQuadMAV.h @@ -11,6 +11,8 @@ public: public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); + /** @brief Send a command to an onboard process */ + void sendProcessCommand(int watchdogId, int processId, unsigned int command); signals: void watchdogReceived(int systemId, int watchdogId, int processCount); void processReceived(int systemId, int watchdogId, int processId, QString name, QString arguments, int timeout); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index a5fbfb9..dc8873f 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -53,7 +53,6 @@ This file is part of the PIXHAWK project #include "ObjectDetectionView.h" #include "HUD.h" #include "PFD.h" -#include "GaugePanel.h" #include "JoystickWidget.h" #include "input/JoystickInput.h" #include "DebugConsole.h" @@ -147,7 +146,6 @@ protected: XMLCommProtocolWidget* protocol; HDDisplay* headDown1; HDDisplay* headDown2; - GaugePanel* gaugePanel; // Popup widgets JoystickWidget* joystickWidget; diff --git a/src/ui/watchdog/WatchdogControl.cc b/src/ui/watchdog/WatchdogControl.cc index 6e0c0a2..0ba5428 100644 --- a/src/ui/watchdog/WatchdogControl.cc +++ b/src/ui/watchdog/WatchdogControl.cc @@ -1,10 +1,12 @@ #include "WatchdogControl.h" #include "ui_WatchdogControl.h" +#include "PxQuadMAV.h" #include -WatchdogControl::WatchdogControl(QWidget *parent) : +WatchdogControl::WatchdogControl(UASInterface* uas, QWidget *parent) : QWidget(parent), + mav(NULL), ui(new Ui::WatchdogControl) { ui->setupUi(this); @@ -15,6 +17,16 @@ WatchdogControl::~WatchdogControl() delete ui; } +void WatchdogControl::setUAS(UASInterface* uas) +{ + PxQuadMAV* qmav = dynamic_cast(uas); + + if (qmav) + { + connect(qmav, SIGNAL(processReceived(int,int,int,QString,QString,int)), this, SLOT(addProcess(int,int,int,QString,QString,int))); + } +} + void WatchdogControl::updateWatchdog(int systemId, int watchdogId, unsigned int processCount) { // request the watchdog with the given ID @@ -101,17 +113,7 @@ WatchdogControl::ProcessInfo& WatchdogControl::WatchdogInfo::getProcess(uint16_t */ 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; + emit sendProcessCommand(w_id.watchdog_id_, p_id, command); } void WatchdogControl::changeEvent(QEvent *e) diff --git a/src/ui/watchdog/WatchdogControl.h b/src/ui/watchdog/WatchdogControl.h index 486dff8..f8988d2 100644 --- a/src/ui/watchdog/WatchdogControl.h +++ b/src/ui/watchdog/WatchdogControl.h @@ -1,6 +1,8 @@ #ifndef WATCHDOGCONTROL_H #define WATCHDOGCONTROL_H +#include + #include #include @@ -8,6 +10,8 @@ #include #include +#include "UASInterface.h" + namespace Ui { class WatchdogControl; } @@ -30,7 +34,7 @@ public: RequestInfo = 254, RequestStatus = 255 - }; + }; }; ///! This struct represents a process on the watchdog. Used to store all values. @@ -46,7 +50,7 @@ public: Stopped = 2, Stopped_OK = 3, Stopped_ERROR = 4 - }; + }; }; ///! Constructor - initialize the values @@ -62,8 +66,8 @@ public: 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) + // 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 @@ -77,7 +81,7 @@ public: ///! 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_); } + { return (this->system_id_ != other.system_id_) ? (this->system_id_ < other.system_id_) : (this->watchdog_id_ < other.watchdog_id_); } }; @@ -90,7 +94,7 @@ public: QTimer* timeoutTimer_; ///< Internal timer, used to measure the time since the last heartbeat message }; - WatchdogControl(QWidget *parent = 0); + WatchdogControl(UASInterface* uas, QWidget *parent = 0); ~WatchdogControl(); static const uint16_t ALL = (uint16_t)-1; ///< A magic value for a process-ID which addresses "all processes" @@ -102,9 +106,14 @@ public slots: 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); +signals: + void sendProcessCommand(int watchdogId, int processId, unsigned int command); + protected: void changeEvent(QEvent *e); + UASInterface* mav; + private: Ui::WatchdogControl *ui; @@ -120,9 +129,9 @@ private: ///! Convert a value to std::string template -std::string convertToString(T value) + std::string convertToString(T value) { -std::ostringstream oss; -oss << value; -return oss.str(); + std::ostringstream oss; + oss << value; + return oss.str(); }