diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index f846c60db2671460785bc69f416384ec14913d81..576108a04e91849cf28d2e648e49b697aecd7209 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -380,7 +380,8 @@ win32-g++ { message(Building for Windows Platform (32bit)) # Special settings for debug - #CONFIG += CONSOLE + CONFIG += CONSOLE + OUTPUT += CONSOLE INCLUDEPATH += $$BASEDIR/lib/sdl/include \ $$BASEDIR/lib/opal/include #\ #\ diff --git a/src/Core.cc b/src/Core.cc index d5846985218c373f6b75a72cdc0730b284a81f47..38a37b93bb6ca8d4d9c2c3869b261d9f5aa062d5 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -65,8 +65,11 @@ This file is part of the QGROUNDCONTROL project * @param argv The string array of parameters **/ + Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) { + + // Set application name this->setApplicationName(QGC_APPLICATION_NAME); this->setApplicationVersion(QGC_APPLICATION_VERSION); diff --git a/src/main.cc b/src/main.cc index a27a8d303027496e62beeebacdf9656dc420cce9..ba6d1af2df48400edd6b96cafd3f3ec17626ab62 100644 --- a/src/main.cc +++ b/src/main.cc @@ -39,6 +39,21 @@ This file is part of the QGROUNDCONTROL project #undef main #endif + +// Install a message handler so you do not need +// the MSFT debug tools installed to se +// qDebug(), qWarning(), qCritical and qAbort +#ifdef Q_OS_WIN +void msgHandler( QtMsgType type, const char* msg ) +{ + const char symbols[] = { 'I', 'E', '!', 'X' }; + QString output = QString("[%1] %2").arg( symbols[type] ).arg( msg ); + std::cerr << output.toStdString() << std::endl; + if( type == QtFatalMsg ) abort(); +} + +#endif + /** * @brief Starts the application * @@ -46,9 +61,15 @@ This file is part of the QGROUNDCONTROL project * @param argv Commandline arguments * @return exit code, 0 for normal exit and !=0 for error cases */ + int main(int argc, char *argv[]) { +// install the message handler +#ifdef Q_OS_WIN + qInstallMsgHandler( msgHandler ); +#endif + Core core(argc, argv); return core.exec(); } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dc8cb9e047eca0828e258d4311c30d1009e043ac..ca8e0518f966bdfb81195d5b7f82519362dd7442 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -244,13 +244,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_sys_status_decode(&message, &state); // FIXME - //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode; + qDebug() << "1 SYSTEM STATUS:" << state.status; QString audiostring = "System " + QString::number(this->getUASID()); QString stateAudio = ""; QString modeAudio = ""; bool statechanged = false; - bool modechanged = false; + bool modechanged = false; if (state.status != this->status) { @@ -264,6 +264,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) stateAudio = " changed status to " + uasState; } + qDebug() << "1 SYSTEM MODE:" << state.mode; + qDebug() << "1 THIS MODE:" << this->mode; if (this->mode != static_cast(state.mode)) { modechanged = true; @@ -305,6 +307,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit modeChanged(this->getUASID(), mode, ""); + + qDebug() << "2 SYSTEM MODE:" << mode; + modeAudio = " is now in " + mode; } currentVoltage = state.vbat/1000.0f; @@ -984,12 +989,16 @@ void UAS::setMode(int mode) { if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) { - this->mode = mode; + //this->mode = mode; //no call assignament, update receive message from UAS mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); sendMessage(msg); qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; } + else + { + qDebug() << "uas Mode not assign: " << mode; + } } void UAS::sendMessage(mavlink_message_t message) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d65d319f00aa942e83f870e9d064983c22fab823..08af3851c6e11d0831a35cc5b338e82cb8ccafb8 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -1,330 +1,333 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL 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. - - QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of Unmanned Aerial Vehicle object - * - * @author Lorenz Meier - * - */ - -#ifndef _UAS_H_ -#define _UAS_H_ - -#include "UASInterface.h" -#include "MG.h" -#include -#include "QGCMAVLink.h" - -/** - * @brief A generic MAVLINK-connected MAV/UAV - * - * This class represents one vehicle. It can be used like the real vehicle, e.g. a call to halt() - * will automatically send the appropriate messages to the vehicle. The vehicle state will also be - * automatically updated by the comm architecture, so when writing code to e.g. control the vehicle - * no knowledge of the communication infrastructure is needed. - */ -class UAS : public UASInterface { - Q_OBJECT -public: - UAS(MAVLinkProtocol* protocol, int id = 0); - ~UAS(); - - enum BatteryType - { - NICD = 0, - NIMH = 1, - LIION = 2, - LIPOLY = 3, - LIFE = 4, - AGZN = 5 - }; ///< The type of battery used - - static const int lipoFull = 4.2f; ///< 100% charged voltage - static const int lipoEmpty = 3.5f; ///< Discharged voltage - - /* MANAGEMENT */ - - /** @brief The name of the robot */ - QString getUASName(void) const; - /** @brief Get the unique system id */ - int getUASID() const; - /** @brief Get the airframe */ - int getAirframe() const { return airframe; } - /** @brief The time interval the robot is switched on */ - quint64 getUptime() const; - /** @brief Get the status flag for the communication */ - int getCommunicationStatus() const; - /** @brief Add one measurement and get low-passed voltage */ - float filterVoltage(float value) const; - /** @brief Get the links associated with this robot */ - QList* getLinks(); - - double getLocalX() const { return localX; } - double getLocalY() const { return localY; } - double getLocalZ() const { return localZ; } - double getLatitude() const { return latitude; } - double getLongitude() const { return longitude; } - double getAltitude() const { return altitude; } - - double getRoll() const { return roll; } - double getPitch() const { return pitch; } - double getYaw() const { return yaw; } - bool getSelected() const; - -friend class UASWaypointManager; - -protected: //COMMENTS FOR TEST UNIT - int uasId; ///< Unique system ID - unsigned char type; ///< UAS type (from type enum) - quint64 startTime; ///< The time the UAS was switched on - CommStatus commStatus; ///< Communication status - QString name; ///< Human-friendly name of the vehicle, e.g. bravo - int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - QList* links; ///< List of links this UAS can be reached by - QList unknownPackets; ///< Packet IDs which are unknown and have been received - MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance - BatteryType batteryType; ///< The battery type - int cells; ///< Number of cells - - UASWaypointManager waypointManager; - - QList actuatorValues; - QList actuatorNames; - - QList motorValues; - QList motorNames; - - double thrustSum; ///< Sum of forward/up thrust of all thrust actuators, in Newtons - double thrustMax; ///< Maximum forward/up thrust of this vehicle, in Newtons - - // Battery stats - double fullVoltage; ///< Voltage of the fully charged battery (100%) - double emptyVoltage; ///< Voltage of the empty battery (0%) - double startVoltage; ///< Voltage at system start - double currentVoltage; ///< Voltage currently measured - float lpVoltage; ///< Low-pass filtered voltage - int timeRemaining; ///< Remaining time calculated based on previous and current - unsigned int mode; ///< The current mode of the MAV - int status; ///< The current status of the MAV - quint64 onboardTimeOffset; - - bool controlRollManual; ///< status flag, true if roll is controlled manually - bool controlPitchManual; ///< status flag, true if pitch is controlled manually - bool controlYawManual; ///< status flag, true if yaw is controlled manually - bool controlThrustManual; ///< status flag, true if thrust is controlled manually - - double manualRollAngle; ///< Roll angle set by human pilot (radians) - double manualPitchAngle; ///< Pitch angle set by human pilot (radians) - double manualYawAngle; ///< Yaw angle set by human pilot (radians) - double manualThrust; ///< Thrust set by human pilot (radians) - float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) - float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS - bool lowBattAlarm; ///< Switch if battery is low - bool positionLock; ///< Status if position information is available or not - double localX; - double localY; - double localZ; - double latitude; - double longitude; - double altitude; - double speedX; ///< True speed in X axis - double speedY; ///< True speed in Y axis - double speedZ; ///< True speed in Z axis - double roll; - double pitch; - double yaw; - quint64 lastHeartbeat; ///< Time of the last heartbeat message - QTimer* statusTimeout; ///< Timer for various status timeouts - QMap* > parameters; ///< All parameters - bool paramsOnceRequested; ///< If the parameter list has been read at least once - int airframe; ///< The airframe type -public: - /** @brief Set the current battery type */ - void setBattery(BatteryType type, int cells); - /** @brief Estimate how much flight time is remaining */ - int calculateTimeRemaining(); - /** @brief Get the current charge level */ - double getChargeLevel(); - /** @brief Get the human-readable status message for this code */ - void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); - /** @brief Check if vehicle is in autonomous mode */ - bool isAuto(); - -public: - UASWaypointManager* getWaypointManager() { return &waypointManager; } - int getSystemType(); - int getAutopilotType() {return autopilot;} - -public slots: - /** @brief Set the autopilot type */ - void setAutopilotType(int apType) { autopilot = apType; emit systemSpecsChanged(uasId); } - /** @brief Set the type of airframe */ - void setSystemType(int systemType) { type = systemType; emit systemSpecsChanged(uasId); } - /** @brief Set the specific airframe type */ - void setAirframe(int airframe) { this->airframe = airframe; emit systemSpecsChanged(uasId); } - /** @brief Set a new name **/ - void setUASName(const QString& name); - /** @brief Executes an action **/ - void setAction(MAV_ACTION action); - - /** @brief Launches the system **/ - void launch(); - /** @brief Write this waypoint to the list of waypoints */ - //void setWaypoint(Waypoint* wp); FIXME tbd - /** @brief Set currently active waypoint */ - //void setWaypointActive(int id); FIXME tbd - /** @brief Order the robot to return home / to land on the runway **/ - void home(); - void halt(); - void go(); - /** @brief Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/ - void emergencySTOP(); - - /** @brief Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/ - bool emergencyKILL(); - - /** @brief Shut the system cleanly down. Will shut down any onboard computers **/ - void shutdown(); - - /** @brief Set the target position for the robot to navigate to. */ - void setTargetPosition(float x, float y, float z, float yaw); - - void startLowBattAlarm(); - void stopLowBattAlarm(); - - //void requestWaypoints(); FIXME tbd - //void clearWaypointList(); FIXME tbd - - /** @brief Enable the motors */ - void enable_motors(); - /** @brief Disable the motors */ - void disable_motors(); - - /** @brief Set the values for the manual control of the vehicle */ - void setManualControlCommands(double roll, double pitch, double yaw, double thrust); - /** @brief Receive a button pressed event from an input device, e.g. joystick */ - void receiveButton(int buttonIndex); - - /** @brief Add a link associated with this robot */ - void addLink(LinkInterface* link); - /** @brief Remove a link associated with this robot */ - void removeLink(QObject* object); - - /** @brief Receive a message from one of the communication links. */ - virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); - - /** @brief Send a message over this link (to this or to all UAS on this link) */ - void sendMessage(LinkInterface* link, mavlink_message_t message); - /** @brief Send a message over all links this UAS can be reached with (!= all links) */ - void sendMessage(mavlink_message_t message); - - /** @brief Temporary Hack for sending packets to patch Antenna. Send a message over all serial links except for this UAS's */ - void forwardMessage(mavlink_message_t message); - - /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ - void setSelected(); - - /** @brief Set current mode of operation, e.g. auto or manual */ - void setMode(int mode); - - /** @brief Request all parameters */ - void requestParameters(); - - /** @brief Set a system parameter */ - void setParameter(const int component, const QString& id, const float value); - - /** @brief Write parameters to permanent storage */ - void writeParametersToStorage(); - /** @brief Read parameters from permanent storage */ - void readParametersFromStorage(); - - /** @brief Get the names of all parameters */ - QList getParameterNames(int component); - - /** @brief Get the ids of all components */ - QList getComponentIds(); - - void enableAllDataTransmission(int rate); - void enableRawSensorDataTransmission(int rate); - void enableExtendedSystemStatusTransmission(int rate); - void enableRCChannelDataTransmission(int rate); - void enableRawControllerDataTransmission(int rate); - void enableRawSensorFusionTransmission(int rate); - void enablePositionTransmission(int rate); - void enableExtra1Transmission(int rate); - void enableExtra2Transmission(int rate); - void enableExtra3Transmission(int rate); - - /** @brief Update the system state */ - void updateState(); - - /** @brief Set world frame origin at current GPS position */ - void setLocalOriginAtCurrentGPSPosition(); - /** @brief Set local position setpoint */ - void setLocalPositionSetpoint(float x, float y, float z, float yaw); - /** @brief Add an offset in body frame to the setpoint */ - void setLocalPositionOffset(float x, float y, float z, float yaw); - - void startRadioControlCalibration(); - void startMagnetometerCalibration(); - void startGyroscopeCalibration(); - void startPressureCalibration(); - - void startDataRecording(); - void pauseDataRecording(); - void stopDataRecording(); - -signals: - - /** @brief The main/battery voltage has changed/was updated */ - void voltageChanged(int uasId, double voltage); - /** @brief An actuator value has changed */ - void actuatorChanged(UASInterface*, int actId, double value); - /** @brief An actuator value has changed */ - void actuatorChanged(UASInterface* uas, QString actuatorName, double min, double max, double value); - void motorChanged(UASInterface* uas, QString motorName, double min, double max, double value); - /** @brief The system load (MCU/CPU usage) changed */ - void loadChanged(UASInterface* uas, double load); - /** @brief Propagate a heartbeat received from the system */ - void heartbeat(UASInterface* uas); - void imageStarted(quint64 timestamp); - - protected: - /** @brief Get the UNIX timestamp in microseconds */ - quint64 getUnixTime(quint64 time); - -protected slots: - /** @brief Write settings to disk */ - void writeSettings(); - /** @brief Read settings from disk */ - void readSettings(); - - // MESSAGE RECEPTION - /** @brief Receive a named value message */ - void receiveMessageNamedValue(const mavlink_message_t& message); -}; - - -#endif // _UAS_H_ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of Unmanned Aerial Vehicle object + * + * @author Lorenz Meier + * + */ + +#ifndef _UAS_H_ +#define _UAS_H_ + +#include "UASInterface.h" +#include "MG.h" +#include +#include "QGCMAVLink.h" + +/** + * @brief A generic MAVLINK-connected MAV/UAV + * + * This class represents one vehicle. It can be used like the real vehicle, e.g. a call to halt() + * will automatically send the appropriate messages to the vehicle. The vehicle state will also be + * automatically updated by the comm architecture, so when writing code to e.g. control the vehicle + * no knowledge of the communication infrastructure is needed. + */ +class UAS : public UASInterface { + Q_OBJECT +public: + UAS(MAVLinkProtocol* protocol, int id = 0); + ~UAS(); + + enum BatteryType + { + NICD = 0, + NIMH = 1, + LIION = 2, + LIPOLY = 3, + LIFE = 4, + AGZN = 5 + }; ///< The type of battery used + + static const int lipoFull = 4.2f; ///< 100% charged voltage + static const int lipoEmpty = 3.5f; ///< Discharged voltage + + /* MANAGEMENT */ + + /** @brief The name of the robot */ + QString getUASName(void) const; + /** @brief Get the unique system id */ + int getUASID() const; + /** @brief Get the airframe */ + int getAirframe() const { return airframe; } + /** @brief The time interval the robot is switched on */ + quint64 getUptime() const; + /** @brief Get the status flag for the communication */ + int getCommunicationStatus() const; + /** @brief Add one measurement and get low-passed voltage */ + float filterVoltage(float value) const; + /** @brief Get the links associated with this robot */ + QList* getLinks(); + + double getLocalX() const { return localX; } + double getLocalY() const { return localY; } + double getLocalZ() const { return localZ; } + double getLatitude() const { return latitude; } + double getLongitude() const { return longitude; } + double getAltitude() const { return altitude; } + + double getRoll() const { return roll; } + double getPitch() const { return pitch; } + double getYaw() const { return yaw; } + bool getSelected() const; + +friend class UASWaypointManager; + +protected: //COMMENTS FOR TEST UNIT + int uasId; ///< Unique system ID + unsigned char type; ///< UAS type (from type enum) + quint64 startTime; ///< The time the UAS was switched on + CommStatus commStatus; ///< Communication status + QString name; ///< Human-friendly name of the vehicle, e.g. bravo + int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + QList* links; ///< List of links this UAS can be reached by + QList unknownPackets; ///< Packet IDs which are unknown and have been received + MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance + BatteryType batteryType; ///< The battery type + int cells; ///< Number of cells + + UASWaypointManager waypointManager; + + QList actuatorValues; + QList actuatorNames; + + QList motorValues; + QList motorNames; + + double thrustSum; ///< Sum of forward/up thrust of all thrust actuators, in Newtons + double thrustMax; ///< Maximum forward/up thrust of this vehicle, in Newtons + + // Battery stats + double fullVoltage; ///< Voltage of the fully charged battery (100%) + double emptyVoltage; ///< Voltage of the empty battery (0%) + double startVoltage; ///< Voltage at system start + double currentVoltage; ///< Voltage currently measured + float lpVoltage; ///< Low-pass filtered voltage + int timeRemaining; ///< Remaining time calculated based on previous and current + //unsigned int mode; ///< The current mode of the MAV + int status; ///< The current status of the MAV + quint64 onboardTimeOffset; + + bool controlRollManual; ///< status flag, true if roll is controlled manually + bool controlPitchManual; ///< status flag, true if pitch is controlled manually + bool controlYawManual; ///< status flag, true if yaw is controlled manually + bool controlThrustManual; ///< status flag, true if thrust is controlled manually + + double manualRollAngle; ///< Roll angle set by human pilot (radians) + double manualPitchAngle; ///< Pitch angle set by human pilot (radians) + double manualYawAngle; ///< Yaw angle set by human pilot (radians) + double manualThrust; ///< Thrust set by human pilot (radians) + float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) + float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS + bool lowBattAlarm; ///< Switch if battery is low + bool positionLock; ///< Status if position information is available or not + double localX; + double localY; + double localZ; + double latitude; + double longitude; + double altitude; + double speedX; ///< True speed in X axis + double speedY; ///< True speed in Y axis + double speedZ; ///< True speed in Z axis + double roll; + double pitch; + double yaw; + quint64 lastHeartbeat; ///< Time of the last heartbeat message + QTimer* statusTimeout; ///< Timer for various status timeouts + QMap* > parameters; ///< All parameters + bool paramsOnceRequested; ///< If the parameter list has been read at least once + int airframe; ///< The airframe type +public: + /** @brief Set the current battery type */ + void setBattery(BatteryType type, int cells); + /** @brief Estimate how much flight time is remaining */ + int calculateTimeRemaining(); + /** @brief Get the current charge level */ + double getChargeLevel(); + /** @brief Get the human-readable status message for this code */ + void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); + /** @brief Check if vehicle is in autonomous mode */ + bool isAuto(); + +public: + UASWaypointManager* getWaypointManager() { return &waypointManager; } + int getSystemType(); + int getAutopilotType() {return autopilot;} + +public slots: + /** @brief Set the autopilot type */ + void setAutopilotType(int apType) { autopilot = apType; emit systemSpecsChanged(uasId); } + /** @brief Set the type of airframe */ + void setSystemType(int systemType) { type = systemType; emit systemSpecsChanged(uasId); } + /** @brief Set the specific airframe type */ + void setAirframe(int airframe) { this->airframe = airframe; emit systemSpecsChanged(uasId); } + /** @brief Set a new name **/ + void setUASName(const QString& name); + /** @brief Executes an action **/ + void setAction(MAV_ACTION action); + + /** @brief Launches the system **/ + void launch(); + /** @brief Write this waypoint to the list of waypoints */ + //void setWaypoint(Waypoint* wp); FIXME tbd + /** @brief Set currently active waypoint */ + //void setWaypointActive(int id); FIXME tbd + /** @brief Order the robot to return home / to land on the runway **/ + void home(); + void halt(); + void go(); + /** @brief Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/ + void emergencySTOP(); + + /** @brief Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/ + bool emergencyKILL(); + + /** @brief Shut the system cleanly down. Will shut down any onboard computers **/ + void shutdown(); + + /** @brief Set the target position for the robot to navigate to. */ + void setTargetPosition(float x, float y, float z, float yaw); + + void startLowBattAlarm(); + void stopLowBattAlarm(); + + //void requestWaypoints(); FIXME tbd + //void clearWaypointList(); FIXME tbd + + /** @brief Enable the motors */ + void enable_motors(); + /** @brief Disable the motors */ + void disable_motors(); + + /** @brief Set the values for the manual control of the vehicle */ + void setManualControlCommands(double roll, double pitch, double yaw, double thrust); + /** @brief Receive a button pressed event from an input device, e.g. joystick */ + void receiveButton(int buttonIndex); + + /** @brief Add a link associated with this robot */ + void addLink(LinkInterface* link); + /** @brief Remove a link associated with this robot */ + void removeLink(QObject* object); + + /** @brief Receive a message from one of the communication links. */ + virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); + + /** @brief Send a message over this link (to this or to all UAS on this link) */ + void sendMessage(LinkInterface* link, mavlink_message_t message); + /** @brief Send a message over all links this UAS can be reached with (!= all links) */ + void sendMessage(mavlink_message_t message); + + /** @brief Temporary Hack for sending packets to patch Antenna. Send a message over all serial links except for this UAS's */ + void forwardMessage(mavlink_message_t message); + + /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ + void setSelected(); + + /** @brief Set current mode of operation, e.g. auto or manual */ + void setMode(int mode); + + /** @brief Request all parameters */ + void requestParameters(); + + /** @brief Set a system parameter */ + void setParameter(const int component, const QString& id, const float value); + + /** @brief Write parameters to permanent storage */ + void writeParametersToStorage(); + /** @brief Read parameters from permanent storage */ + void readParametersFromStorage(); + + /** @brief Get the names of all parameters */ + QList getParameterNames(int component); + + /** @brief Get the ids of all components */ + QList getComponentIds(); + + void enableAllDataTransmission(int rate); + void enableRawSensorDataTransmission(int rate); + void enableExtendedSystemStatusTransmission(int rate); + void enableRCChannelDataTransmission(int rate); + void enableRawControllerDataTransmission(int rate); + void enableRawSensorFusionTransmission(int rate); + void enablePositionTransmission(int rate); + void enableExtra1Transmission(int rate); + void enableExtra2Transmission(int rate); + void enableExtra3Transmission(int rate); + + /** @brief Update the system state */ + void updateState(); + + /** @brief Set world frame origin at current GPS position */ + void setLocalOriginAtCurrentGPSPosition(); + /** @brief Set local position setpoint */ + void setLocalPositionSetpoint(float x, float y, float z, float yaw); + /** @brief Add an offset in body frame to the setpoint */ + void setLocalPositionOffset(float x, float y, float z, float yaw); + + void startRadioControlCalibration(); + void startMagnetometerCalibration(); + void startGyroscopeCalibration(); + void startPressureCalibration(); + + void startDataRecording(); + void pauseDataRecording(); + void stopDataRecording(); + +signals: + + /** @brief The main/battery voltage has changed/was updated */ + void voltageChanged(int uasId, double voltage); + /** @brief An actuator value has changed */ + void actuatorChanged(UASInterface*, int actId, double value); + /** @brief An actuator value has changed */ + void actuatorChanged(UASInterface* uas, QString actuatorName, double min, double max, double value); + void motorChanged(UASInterface* uas, QString motorName, double min, double max, double value); + /** @brief The system load (MCU/CPU usage) changed */ + void loadChanged(UASInterface* uas, double load); + /** @brief Propagate a heartbeat received from the system */ + void heartbeat(UASInterface* uas); + void imageStarted(quint64 timestamp); + + protected: + /** @brief Get the UNIX timestamp in microseconds */ + quint64 getUnixTime(quint64 time); + +protected slots: + /** @brief Write settings to disk */ + void writeSettings(); + /** @brief Read settings from disk */ + void readSettings(); + + // MESSAGE RECEPTION + /** @brief Receive a named value message */ + void receiveMessageNamedValue(const mavlink_message_t& message); + +private: + unsigned int mode; ///< The current mode of the MAV +}; + + +#endif // _UAS_H_ diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 359c7a96f77aec305f0e8cb14269e501547af597..968e028f1c1dee72a6e38cbd3d0e61c988db283e 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -1,293 +1,294 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009, 2010 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 Definition of widget controlling one MAV - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include "UASControlWidget.h" -#include -#include - -#define CONTROL_MODE_LOCKED "MODE LOCKED" -#define CONTROL_MODE_MANUAL "MODE MANUAL" -#define CONTROL_MODE_GUIDED "MODE GUIDED" -#define CONTROL_MODE_AUTO "MODE AUTO" -#define CONTROL_MODE_TEST1 "MODE TEST1" -#define CONTROL_MODE_TEST2 "MODE TEST2" -#define CONTROL_MODE_TEST3 "MODE TEST3" -#define CONTROL_MODE_READY "MODE TEST3" -#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" - -#define CONTROL_MODE_LOCKED_INDEX 1 -#define CONTROL_MODE_MANUAL_INDEX 2 -#define CONTROL_MODE_GUIDED_INDEX 3 -#define CONTROL_MODE_AUTO_INDEX 4 -#define CONTROL_MODE_TEST1_INDEX 5 -#define CONTROL_MODE_TEST2_INDEX 6 -#define CONTROL_MODE_TEST3_INDEX 7 -#define CONTROL_MODE_READY_INDEX 8 -#define CONTROL_MODE_RC_TRAINING_INDEX 9 - -UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), -uas(0), -engineOn(false) -{ - ui.setupUi(this); - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - ui.modeComboBox->clear(); - ui.modeComboBox->insertItem(0, "Select.."); - ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED); - ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL); - ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED); - ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3); - ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY); - ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING); - connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); - connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); - - ui.modeComboBox->setCurrentIndex(0); - - ui.gridLayout->setAlignment(Qt::AlignTop); -} - -void UASControlWidget::setUAS(UASInterface* uas) -{ - if (this->uas != 0) - { - UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas); - disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(enable_motors())); - disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); - disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); - disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); - //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); - disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - } - - // Connect user interface controls - connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); - connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); - connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); - connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); - //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); - connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - - ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); - -// // Check if additional controls should be loaded -// UAS* mav = dynamic_cast(uas); -// if (mav) -// { -// QPushButton* startRecButton = new QPushButton(tr("Record")); -// connect(startRecButton, SIGNAL(clicked()), mav, SLOT(startDataRecording())); -// ui.gridLayout->addWidget(startRecButton, 7, 1); - -// QPushButton* pauseRecButton = new QPushButton(tr("Pause")); -// connect(pauseRecButton, SIGNAL(clicked()), mav, SLOT(pauseDataRecording())); -// ui.gridLayout->addWidget(pauseRecButton, 7, 3); - -// QPushButton* stopRecButton = new QPushButton(tr("Stop")); -// connect(stopRecButton, SIGNAL(clicked()), mav, SLOT(stopDataRecording())); -// ui.gridLayout->addWidget(stopRecButton, 7, 4); -// } - - - this->uas = uas->getUASID(); - setBackgroundColor(uas->getColor()); -} - -UASControlWidget::~UASControlWidget() -{ - -} - -void UASControlWidget::updateStatemachine() -{ - - if (engineOn) - { - ui.controlButton->setText(tr("Stop Engine")); - } - else - { - ui.controlButton->setText(tr("Activate Engine")); - } -} - -/** - * Set the background color based on the MAV color. If the MAV is selected as the - * currently actively controlled system, the frame color is highlighted - */ -void UASControlWidget::setBackgroundColor(QColor color) -{ - // UAS color - QColor uasColor = color; - QString colorstyle; - QString borderColor = "#4A4A4F"; - borderColor = "#FA4A4F"; - uasColor = uasColor.darker(900); - colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }", - uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); - setStyleSheet(colorstyle); - QPalette palette = this->palette(); - palette.setBrush(QPalette::Window, QBrush(uasColor)); - setPalette(palette); - setAutoFillBackground(true); -} - - -void UASControlWidget::updateMode(int uas,QString mode,QString description) -{ - Q_UNUSED(uas); - Q_UNUSED(mode); - Q_UNUSED(description); -} - -void UASControlWidget::updateState(int state) -{ - switch (state) - { - case (int)MAV_STATE_ACTIVE: - engineOn = true; - ui.controlButton->setText(tr("Stop Engine")); - break; - case (int)MAV_STATE_STANDBY: - engineOn = false; - ui.controlButton->setText(tr("Activate Engine")); - break; - } -} - -void UASControlWidget::setMode(int mode) -{ - // Adapt context button mode - if (mode == CONTROL_MODE_LOCKED_INDEX) - { - uasMode = (unsigned int)MAV_MODE_LOCKED; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_MANUAL_INDEX) - { - uasMode = (unsigned int)MAV_MODE_MANUAL; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_GUIDED_INDEX) - { - uasMode = (unsigned int)MAV_MODE_GUIDED; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_AUTO_INDEX) - { - uasMode = (unsigned int)MAV_MODE_AUTO; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST1_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST1; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST2_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST2; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST3_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST3; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_RC_TRAINING_INDEX) - { - uasMode = (unsigned int)MAV_MODE_RC_TRAINING; - ui.modeComboBox->setCurrentIndex(mode); - } - else - { - qDebug() << "ERROR! MODE NOT FOUND"; - uasMode = 0; - } - - - qDebug() << "SET MODE REQUESTED" << uasMode; -} - -void UASControlWidget::transmitMode() -{ - if (uasMode != 0) - { - UASInterface* mav = UASManager::instance()->getUASForId(this->uas); - if (mav) - { - mav->setMode(uasMode); - ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(mav->getUASName())); - } - } -} - -void UASControlWidget::cycleContextButton() -{ - UAS* mav = dynamic_cast(UASManager::instance()->getUASForId(this->uas)); - if (mav) - { - - if (!engineOn) - { - mav->enable_motors(); - ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName())); - } - else - { - mav->disable_motors(); - ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName())); - } - // Update state now and in several intervals when MAV might have changed state - updateStatemachine(); - - QTimer::singleShot(50, this, SLOT(updateStatemachine())); - QTimer::singleShot(200, this, SLOT(updateStatemachine())); - - //ui.controlButton->setText(tr("Force Landing")); - //ui.controlButton->setText(tr("KILL VEHICLE")); - } - -} - +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 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 Definition of widget controlling one MAV + * + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "UASControlWidget.h" +#include +#include +#include "QGC.h" + +#define CONTROL_MODE_LOCKED "MODE LOCKED" +#define CONTROL_MODE_MANUAL "MODE MANUAL" +#define CONTROL_MODE_GUIDED "MODE GUIDED" +#define CONTROL_MODE_AUTO "MODE AUTO" +#define CONTROL_MODE_TEST1 "MODE TEST1" +#define CONTROL_MODE_TEST2 "MODE TEST2" +#define CONTROL_MODE_TEST3 "MODE TEST3" +#define CONTROL_MODE_READY "MODE TEST3" +#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" + +#define CONTROL_MODE_LOCKED_INDEX 1 +#define CONTROL_MODE_MANUAL_INDEX 2 +#define CONTROL_MODE_GUIDED_INDEX 3 +#define CONTROL_MODE_AUTO_INDEX 4 +#define CONTROL_MODE_TEST1_INDEX 5 +#define CONTROL_MODE_TEST2_INDEX 6 +#define CONTROL_MODE_TEST3_INDEX 7 +#define CONTROL_MODE_READY_INDEX 8 +#define CONTROL_MODE_RC_TRAINING_INDEX 9 + +UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), +uas(0), +engineOn(false) +{ + ui.setupUi(this); + + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + ui.modeComboBox->clear(); + ui.modeComboBox->insertItem(0, "Select.."); + ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED); + ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL); + ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED); + ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3); + ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY); + ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING); + connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); + connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); + + ui.modeComboBox->setCurrentIndex(0); + + ui.gridLayout->setAlignment(Qt::AlignTop); +} + +void UASControlWidget::setUAS(UASInterface* uas) +{ + if (this->uas != 0) + { + UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas); + disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(enable_motors())); + disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); + disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); + disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); + //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); + disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + } + + // Connect user interface controls + connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); + connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); + connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); + connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); + //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); + connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + + ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); + +// // Check if additional controls should be loaded +// UAS* mav = dynamic_cast(uas); +// if (mav) +// { +// QPushButton* startRecButton = new QPushButton(tr("Record")); +// connect(startRecButton, SIGNAL(clicked()), mav, SLOT(startDataRecording())); +// ui.gridLayout->addWidget(startRecButton, 7, 1); + +// QPushButton* pauseRecButton = new QPushButton(tr("Pause")); +// connect(pauseRecButton, SIGNAL(clicked()), mav, SLOT(pauseDataRecording())); +// ui.gridLayout->addWidget(pauseRecButton, 7, 3); + +// QPushButton* stopRecButton = new QPushButton(tr("Stop")); +// connect(stopRecButton, SIGNAL(clicked()), mav, SLOT(stopDataRecording())); +// ui.gridLayout->addWidget(stopRecButton, 7, 4); +// } + + + this->uas = uas->getUASID(); + setBackgroundColor(uas->getColor()); +} + +UASControlWidget::~UASControlWidget() +{ + +} + +void UASControlWidget::updateStatemachine() +{ + + if (engineOn) + { + ui.controlButton->setText(tr("Stop Engine")); + } + else + { + ui.controlButton->setText(tr("Activate Engine")); + } +} + +/** + * Set the background color based on the MAV color. If the MAV is selected as the + * currently actively controlled system, the frame color is highlighted + */ +void UASControlWidget::setBackgroundColor(QColor color) +{ + // UAS color + QColor uasColor = color; + QString colorstyle; + QString borderColor = "#4A4A4F"; + borderColor = "#FA4A4F"; + uasColor = uasColor.darker(900); + colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }", + uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); + setStyleSheet(colorstyle); + QPalette palette = this->palette(); + palette.setBrush(QPalette::Window, QBrush(uasColor)); + setPalette(palette); + setAutoFillBackground(true); +} + + +void UASControlWidget::updateMode(int uas,QString mode,QString description) +{ + Q_UNUSED(uas); + Q_UNUSED(mode); + Q_UNUSED(description); +} + +void UASControlWidget::updateState(int state) +{ + switch (state) + { + case (int)MAV_STATE_ACTIVE: + engineOn = true; + ui.controlButton->setText(tr("Stop Engine")); + break; + case (int)MAV_STATE_STANDBY: + engineOn = false; + ui.controlButton->setText(tr("Activate Engine")); + break; + } +} + +void UASControlWidget::setMode(int mode) +{ + // Adapt context button mode + if (mode == CONTROL_MODE_LOCKED_INDEX) + { + uasMode = (unsigned int)MAV_MODE_LOCKED; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_MANUAL_INDEX) + { + uasMode = (unsigned int)MAV_MODE_MANUAL; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_GUIDED_INDEX) + { + uasMode = (unsigned int)MAV_MODE_GUIDED; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_AUTO_INDEX) + { + uasMode = (unsigned int)MAV_MODE_AUTO; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST1_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST1; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST2_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST2; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST3_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST3; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_RC_TRAINING_INDEX) + { + uasMode = (unsigned int)MAV_MODE_RC_TRAINING; + ui.modeComboBox->setCurrentIndex(mode); + } + else + { + qDebug() << "ERROR! MODE NOT FOUND"; + uasMode = 0; + } + + + qDebug() << "SET MODE REQUESTED" << uasMode; +} + +void UASControlWidget::transmitMode() +{ + if (uasMode != 0) + { + UASInterface* mav = UASManager::instance()->getUASForId(this->uas); + if (mav) + { + mav->setMode(uasMode); + ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(mav->getUASName())); + } + } +} + +void UASControlWidget::cycleContextButton() +{ + UAS* mav = dynamic_cast(UASManager::instance()->getUASForId(this->uas)); + if (mav) + { + + if (!engineOn) + { + mav->enable_motors(); + ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName())); + } + else + { + mav->disable_motors(); + ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName())); + } + // Update state now and in several intervals when MAV might have changed state + updateStatemachine(); + + QTimer::singleShot(50, this, SLOT(updateStatemachine())); + QTimer::singleShot(200, this, SLOT(updateStatemachine())); + + //ui.controlButton->setText(tr("Force Landing")); + //ui.controlButton->setText(tr("KILL VEHICLE")); + } + +} +