From 2d8796a92addab80c927e30d943755cdc2a3e4f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 13:34:17 +0200 Subject: [PATCH] UAS threading improvements --- src/uas/ArduPilotMegaMAV.cc | 4 ++-- src/uas/ArduPilotMegaMAV.h | 2 +- src/uas/PxQuadMAV.cc | 4 ++-- src/uas/PxQuadMAV.h | 2 +- src/uas/QGCMAVLinkUASFactory.cc | 14 ++++++------- src/uas/SlugsMAV.cc | 4 ++-- src/uas/SlugsMAV.h | 2 +- src/uas/UAS.cc | 37 +++++++-------------------------- src/uas/UAS.h | 3 ++- src/uas/senseSoarMAV.cpp | 4 ++-- src/uas/senseSoarMAV.h | 2 +- 11 files changed, 27 insertions(+), 51 deletions(-) diff --git a/src/uas/ArduPilotMegaMAV.cc b/src/uas/ArduPilotMegaMAV.cc index eaabfc34a..e8b61c697 100644 --- a/src/uas/ArduPilotMegaMAV.cc +++ b/src/uas/ArduPilotMegaMAV.cc @@ -38,8 +38,8 @@ This file is part of the QGROUNDCONTROL project #endif #endif -ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id) : - UAS(mavlink, id)//, +ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, QThread* thread, int id) : + UAS(mavlink, thread, id)//, // place other initializers here { //This does not seem to work. Manually request each stream type at a specified rate. diff --git a/src/uas/ArduPilotMegaMAV.h b/src/uas/ArduPilotMegaMAV.h index 2c3990f4c..1ff5bf57e 100644 --- a/src/uas/ArduPilotMegaMAV.h +++ b/src/uas/ArduPilotMegaMAV.h @@ -29,7 +29,7 @@ class ArduPilotMegaMAV : public UAS { Q_OBJECT public: - ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id = 0); + ArduPilotMegaMAV(MAVLinkProtocol* mavlink, QThread* thread, int id = 0); /** @brief Set camera mount stabilization modes */ void setMountConfigure(unsigned char mode, bool stabilize_roll,bool stabilize_pitch,bool stabilize_yaw); /** @brief Set camera mount control */ diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 0629924bb..337a83ee0 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -23,8 +23,8 @@ This file is part of the QGROUNDCONTROL project #include "PxQuadMAV.h" #include "GAudioOutput.h" -PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) : - UAS(mavlink, id) +PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, QThread* thread, int id) : + UAS(mavlink, thread, id) { } diff --git a/src/uas/PxQuadMAV.h b/src/uas/PxQuadMAV.h index 212c8067f..28c3ba3a6 100644 --- a/src/uas/PxQuadMAV.h +++ b/src/uas/PxQuadMAV.h @@ -31,7 +31,7 @@ class PxQuadMAV : public UAS Q_OBJECT Q_INTERFACES(UASInterface) public: - PxQuadMAV(MAVLinkProtocol* mavlink, int id); + PxQuadMAV(MAVLinkProtocol* mavlink, QThread* thread, int id); public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index a5b183306..b7658ae55 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -27,7 +27,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte { case MAV_AUTOPILOT_GENERIC: { - UAS* mav = new UAS(mavlink, sysid); + UAS* mav = new UAS(mavlink, worker, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -43,7 +43,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte break; case MAV_AUTOPILOT_PIXHAWK: { - PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid); + PxQuadMAV* mav = new PxQuadMAV(mavlink, worker, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -62,7 +62,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte break; case MAV_AUTOPILOT_SLUGS: { - SlugsMAV* mav = new SlugsMAV(mavlink, sysid); + SlugsMAV* mav = new SlugsMAV(mavlink, worker, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -78,7 +78,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte break; case MAV_AUTOPILOT_ARDUPILOTMEGA: { - ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid); + ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, worker, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -95,7 +95,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte #ifdef QGC_USE_SENSESOAR_MESSAGES case MAV_AUTOPILOT_SENSESOAR: { - senseSoarMAV* mav = new senseSoarMAV(mavlink,sysid); + senseSoarMAV* mav = new senseSoarMAV(mavlink,worker, sysid); mav->setSystemType((int)heartbeat->type); mav->moveToThread(worker); @@ -107,11 +107,9 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte #endif default: { - UAS* mav = new UAS(mavlink, sysid); + UAS* mav = new UAS(mavlink, worker, sysid); mav->setSystemType((int)heartbeat->type); - mav->moveToThread(worker); - // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index cd879cd90..047bfea71 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -2,8 +2,8 @@ #include -SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : - UAS(mavlink, id) +SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, QThread* thread, int id) : + UAS(mavlink, thread, id) { widgetTimer = new QTimer (this); widgetTimer->setInterval(SLUGS_UPDATE_RATE); diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index d3e7ae8c7..da88cbea1 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -53,7 +53,7 @@ class SlugsMAV : public UAS public: - SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); + SlugsMAV(MAVLinkProtocol* mavlink, QThread* thread, int id = 0); public slots: /** @brief Receive a MAVLink message from this MAV */ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 020516b52..0ff565db2 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -41,7 +41,7 @@ * creating the UAS. */ -UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), +UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(), lipoFull(4.2f), lipoEmpty(3.5f), uasId(id), @@ -166,6 +166,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), lastSendTimeGPS(0), lastSendTimeSensors(0) { + moveToThread(thread); + waypointManager.moveToThread(thread); + paramMgr.moveToThread(thread); + statusTimeout->moveToThread(thread); + for (unsigned int i = 0; i<255;++i) { componentID[i] = -1; @@ -173,7 +178,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), } // Store a list of available actions for this UAS. - // Basically everything exposted as a SLOT with no return value or arguments. + // Basically everything exposed as a SLOT with no return value or arguments. QAction* newAction = new QAction(tr("Arm"), this); newAction->setToolTip(tr("Enable the UAS so that all actuators are online")); @@ -364,34 +369,6 @@ void UAS::updateState() GAudioOutput::instance()->notifyNegative(); } } - -//#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0 -//#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1 -//#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2 -//#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3 -//#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4 -//#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 - -//#warning THIS IS A HUGE HACK AND SHOULD NEVER SHOW UP IN ANY GIT REPOSITORY -// mavlink_message_t message; - -// mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t sp; - -// sp.group = 0; - -// /* set rate mode, set zero rates and 20% throttle */ -// sp.mode = MAVLINK_OFFBOARD_CONTROL_MODE_RATES | MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED; - -// sp.roll[0] = INT16_MAX * 0.0f; -// sp.pitch[0] = INT16_MAX * 0.0f; -// sp.yaw[0] = INT16_MAX * 0.0f; -// sp.thrust[0] = UINT16_MAX * 0.3f; - - -// /* send from system 200 and component 0 */ -// mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(200, 0, &message, &sp); - -// sendMessage(message); } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index ed90a1b5c..6c4c45cb6 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #ifndef _UAS_H_ #define _UAS_H_ +#include #include "UASInterface.h" #include #include @@ -55,7 +56,7 @@ class UAS : public UASInterface { Q_OBJECT public: - UAS(MAVLinkProtocol* protocol, int id = 0); + UAS(MAVLinkProtocol* protocol, QThread* thread, int id = 0); ~UAS(); float lipoFull; ///< 100% charged voltage diff --git a/src/uas/senseSoarMAV.cpp b/src/uas/senseSoarMAV.cpp index fee483862..234a16be0 100644 --- a/src/uas/senseSoarMAV.cpp +++ b/src/uas/senseSoarMAV.cpp @@ -2,8 +2,8 @@ #include #include -senseSoarMAV::senseSoarMAV(MAVLinkProtocol* mavlink, int id) - : UAS(mavlink, id), senseSoarState(0) +senseSoarMAV::senseSoarMAV(MAVLinkProtocol* mavlink, QThread* thread, int id) + : UAS(mavlink, thread, id), senseSoarState(0) { } diff --git a/src/uas/senseSoarMAV.h b/src/uas/senseSoarMAV.h index faf9013c1..0076eb1d6 100644 --- a/src/uas/senseSoarMAV.h +++ b/src/uas/senseSoarMAV.h @@ -14,7 +14,7 @@ class senseSoarMAV : public UAS Q_INTERFACES(UASInterface) public: - senseSoarMAV(MAVLinkProtocol* mavlink, int id); + senseSoarMAV(MAVLinkProtocol* mavlink, QThread* thread, int id); ~senseSoarMAV(void); public slots: /** @brief Receive a MAVLink message from this MAV */ -- 2.22.0