diff --git a/qgcunittest.pro b/qgcunittest.pro index 0c28c6c84861e4654411f23d05eb1443b99c15e4..498ea58a7cada5161ba7dee0a5843438aef19e3e 100644 --- a/qgcunittest.pro +++ b/qgcunittest.pro @@ -114,7 +114,8 @@ SOURCES += src/uas/UAS.cc \ src/comm/SerialLink.cc \ $$TESTDIR/SlugsMavUnitTest.cc \ $$TESTDIR/testSuite.cc \ - $$TESTDIR/UASUnitTest.cc + $$TESTDIR/UASUnitTest.cc \ + src/uas/QGCMAVLinkUASFactory.cc HEADERS += src/uas/UASInterface.h \ @@ -136,7 +137,8 @@ HEADERS += src/uas/UASInterface.h \ src/comm/SerialLink.h \ $$TESTDIR//SlugsMavUnitTest.h \ $$TESTDIR/AutoTest.h \ - $$TESTDIR/UASUnitTest.h + $$TESTDIR/UASUnitTest.h \ + src/uas/QGCMAVLinkUASFactory.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index a1559eeef6a2e0b5ee875577282dea4232a5f333..13ff2dc06f452008305730b463382a5dcd452e27 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -262,7 +262,8 @@ HEADERS += src/MG.h \ src/ui/designer/QGCToolWidgetItem.h \ src/ui/QGCMAVLinkLogPlayer.h \ src/comm/MAVLinkSimulationWaypointPlanner.h \ - src/comm/MAVLinkSimulationMAV.h + src/comm/MAVLinkSimulationMAV.h \ + src/uas/QGCMAVLinkUASFactory.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008: { @@ -385,7 +386,8 @@ SOURCES += src/main.cc \ src/ui/designer/QGCToolWidgetItem.cc \ src/ui/QGCMAVLinkLogPlayer.cc \ src/comm/MAVLinkSimulationWaypointPlanner.cc \ - src/comm/MAVLinkSimulationMAV.cc + src/comm/MAVLinkSimulationMAV.cc \ + src/uas/QGCMAVLinkUASFactory.cc \ macx|win32-msvc2008: { SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index c672ef94ce99d30a5b40d05d7a9d0ac9aba384a7..641f2117036019e7eeeffdad5826c1e62c4d6629 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -153,7 +153,7 @@ void GAudioOutput::mute(bool mute) { this->muted = mute; QSettings settings; - settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted); + settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted); settings.sync(); } diff --git a/src/Waypoint.cc b/src/Waypoint.cc index c36ec60a0b69699277212fad63b0d0b79dc7ffbe..782253e06ea1625600b5225df3569fa758a7a120 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -55,7 +55,12 @@ Waypoint::~Waypoint() void Waypoint::save(QTextStream &saveStream) { - saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << this->getX() << "\t" << this->getY() << "\t" << this->getZ() << "\t" << this->getYaw() << "\t" << this->getAutoContinue() << "\r\n"; + QString position("%1\t%2\t%3\t%4"); + position = position.arg(x, 0, 'g', 18); + position = position.arg(y, 0, 'g', 18); + position = position.arg(z, 0, 'g', 18); + position = position.arg(yaw, 0, 'g', 8); + saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; } bool Waypoint::load(QTextStream &loadStream) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 187681fc0f9fcbac098a152b20b48e3db86f1e1d..57ac970bb25f97bb66849b72fdc0fca32008dafa 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -27,7 +27,8 @@ #include "configuration.h" #include "LinkManager.h" //#include "MainWindow.h" -#include +#include "QGCMAVLink.h" +#include "QGCMAVLinkUASFactory.h" #include "QGC.h" /** @@ -169,63 +170,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) continue; } - switch (heartbeat.autopilot) - { - case MAV_AUTOPILOT_GENERIC: - - uas = new UAS(this, message.sysid); - - // Connect this robot to the UAS object - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - break; - case MAV_AUTOPILOT_PIXHAWK: - { - // Fixme differentiate between quadrotor and coaxial here - PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); - // 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 - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } - break; - case MAV_AUTOPILOT_SLUGS: - { - SlugsMAV* mav = new SlugsMAV(this, message.sysid); - // 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 - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } - break; - case MAV_AUTOPILOT_ARDUPILOTMEGA: - { - ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); - // 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 - // packets never reach their goal) - connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); - uas = mav; - } - break; - default: - uas = new UAS(this, message.sysid); - break; - } - - // Set the autopilot type - uas->setAutopilotType((int)heartbeat.autopilot); - - // Make UAS aware that this link can be used to communicate with the actual robot - uas->addLink(link); - - // Now add UAS to "official" list, which makes the whole application aware of it - UASManager::instance()->addUAS(uas); - + // Create a new UAS object + uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat); } // Only count message if UAS exists for this message diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index c4e86e77d137e1abee1c4bd9e814933b465b640e..17c664dcbbf52c5d64f10477d29f2c71a2090e83 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -153,14 +153,14 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); // Pack to link buffer - readyBufferMutex.lock(); +// readyBufferMutex.lock(); for (unsigned int i = 0; i < bufferlength; i++) { readyBuffer.enqueue(*(buf + i)); } - readyBufferMutex.unlock(); +// readyBufferMutex.unlock(); - //qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid; + qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid; } void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 7880b57281a9c1b869837c7277f51f2c3e6a9444..80177415555b3550d60b3f0339c421b7254890fb 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -1,4 +1,5 @@ #include +#include #include "MAVLinkSimulationMAV.h" @@ -6,28 +7,126 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy QObject(parent), link(parent), planner(parent, systemid), - systemid(systemid) + systemid(systemid), + timer25Hz(0), + timer10Hz(0), + timer1Hz(0), + latitude(0.0), + longitude(0.0), + altitude(0.0), + x(0.0), + y(0.0), + z(0.0), + roll(0.0), + pitch(0.0), + yaw(0.0), + globalNavigation(true), + firstWP(false), + previousSPX(0.0), + previousSPY(0.0), + previousSPZ(0.0), + previousSPYaw(0.0), + nextSPX(0.0), + nextSPY(0.0), + nextSPZ(0.0), + nextSPYaw(0.0) { + // Please note: The waypoint planner is running connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); - mainloopTimer.start(1000); + connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); + mainloopTimer.start(20); mainloop(); } void MAVLinkSimulationMAV::mainloop() { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); - link->sendMAVLinkMessage(&msg); + // Calculate new simulator values +// double maxSpeed = 0.0001; // rad/s in earth coordinate frame + +// double xNew = // (nextSPX - previousSPX) + + if (!firstWP) + { + x += (nextSPX - previousSPX) * 0.01; + y += (nextSPY - previousSPY) * 0.01; + z += (nextSPZ - previousSPZ) * 0.1; + } + else + { + x = nextSPX; + y = nextSPY; + z = nextSPZ; + firstWP = false; + } + + // 1 Hz execution + if (timer1Hz <= 0) + { + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); + link->sendMAVLinkMessage(&msg); + timer1Hz = 50; + } + + // 10 Hz execution + if (timer10Hz <= 0) + { + mavlink_message_t msg; + mavlink_global_position_int_t pos; + pos.alt = z*1000.0; + pos.lat = y*1E7; + pos.lon = x*1E7; + pos.vx = 0; + pos.vy = 0; + pos.vz = 0; + mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); + link->sendMAVLinkMessage(&msg); + timer10Hz = 5; + } + + // 25 Hz execution + if (timer25Hz <= 0) + { + timer25Hz = 2; + } + + timer1Hz--; + timer10Hz--; + timer25Hz--; } void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) { - qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; + //qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; switch(msg.msgid) { case MAVLINK_MSG_ID_ATTITUDE: break; + case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: + { + mavlink_local_position_setpoint_set_t sp; + mavlink_msg_local_position_setpoint_set_decode(&msg, &sp); + if (sp.target_system == this->systemid) + { + previousSPX = nextSPX; + previousSPY = nextSPY; + previousSPZ = nextSPZ; + nextSPX = sp.x; + nextSPY = sp.y; + nextSPZ = sp.z; + + // Rotary wing + //nextSPYaw = sp.yaw; + + // Airplane + yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY); + + if (!firstWP) firstWP = true; + } + //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ; + } + break; } } diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h index eb9f80e8c5a3b961150597c36b81c466e17d4309..5f92559c93c0e5f562a4ec9645e8897f1bf25b57 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -24,7 +24,31 @@ protected: MAVLinkSimulationWaypointPlanner planner; int systemid; QTimer mainloopTimer; - + int timer25Hz; + int timer10Hz; + int timer1Hz; + double latitude; + double longitude; + double altitude; + double x; + double y; + double z; + double roll; + double pitch; + double yaw; + + bool globalNavigation; + bool firstWP; + + double previousSPX; + double previousSPY; + double previousSPZ; + double previousSPYaw; + + double nextSPX; + double nextSPY; + double nextSPZ; + double nextSPYaw; }; diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index b35d7a3dc3d1a1cda45e1839e2a0e1c9dfef4ee5..43d1bb3cb93950ad9ad45930840d9ba798b70b05 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -34,6 +34,7 @@ This file is part of the PIXHAWK project #include "MAVLinkSimulationWaypointPlanner.h" #include "QGC.h" +#include #ifndef M_PI #define M_PI 3.14159265358979323846 @@ -225,7 +226,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid - if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); } /* @@ -253,7 +254,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) - if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); + if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq); } } @@ -291,10 +292,20 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) } else { - if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + //if (verbose) qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + PControlSetPoint.target_system = systemid; + PControlSetPoint.target_component = MAV_COMP_ID_IMU; + PControlSetPoint.x = cur->x; + PControlSetPoint.y = cur->y; + PControlSetPoint.z = cur->z; + PControlSetPoint.yaw = cur->yaw; + + mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); + link->sendMAVLinkMessage(&msg); + emit messageSent(msg); } - uint64_t now = QGC::groundTimeUsecs(); + uint64_t now = QGC::groundTimeUsecs()/1000; timestamp_last_send_setpoint = now; } } @@ -311,7 +322,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_system mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); link->sendMAVLinkMessage(&msg); - if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); } @@ -325,17 +336,17 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui wp->target_system = target_systemid; wp->target_component = target_compid; - if (verbose) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); + if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); link->sendMAVLinkMessage(&msg); - if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); } else { - if (verbose) printf("ERROR: index out of bounds\n"); + if (verbose) qDebug("ERROR: index out of bounds\n"); } } @@ -348,7 +359,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_syst wpr.seq = seq; mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); link->sendMAVLinkMessage(&msg); - if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); } @@ -370,7 +381,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq) mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); link->sendMAVLinkMessage(&msg); - if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq); } @@ -438,12 +449,12 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //check for timed-out operations - printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid); + qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid; - uint64_t now = QGC::groundTimeUsecs(); + uint64_t now = QGC::groundTimeUsecs()/1000; if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) { - if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); + if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state; current_state = PX_WPP_IDLE; protocol_current_count = 0; protocol_current_partner_systemid = 0; @@ -506,7 +517,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { mavlink_local_position_t pos; mavlink_msg_local_position_decode(msg, &pos); - if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z; posReached = false; @@ -538,7 +549,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* mavlink_msg_action_decode(msg, &action); if(action.target == systemid) { - if (verbose) printf("Waypoint: received message with action %d\n", action.action); + if (verbose) qDebug("Waypoint: received message with action %d\n", action.action); switch (action.action) { // case MAV_ACTION_LAUNCH: @@ -585,7 +596,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { if (protocol_current_wp_id == waypoints->size()-1) { - if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); + if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); current_state = PX_WPP_IDLE; protocol_current_wp_id = 0; } @@ -607,7 +618,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { if (wpc.seq < waypoints->size()) { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + if (verbose) qDebug("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); current_active_wp_id = wpc.seq; uint32_t i; for(i = 0; i < waypoints->size(); i++) @@ -621,7 +632,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* waypoints->at(i)->current = false; } } - if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); yawReached = false; posReached = false; send_waypoint_current(current_active_wp_id); @@ -630,7 +641,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); } } } @@ -649,8 +660,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { if (waypoints->size() > 0) { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); + if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); current_state = PX_WPP_SENDLIST; protocol_current_wp_id = 0; protocol_current_partner_systemid = msg->sysid; @@ -658,19 +669,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* } else { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); } protocol_current_count = waypoints->size(); send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); } } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n"); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n"); } break; } @@ -686,9 +697,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) { - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); current_state = PX_WPP_SENDLIST_SENDWPS; protocol_current_wp_id = wpr.seq; @@ -698,17 +709,17 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { if (verbose) { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } + if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } else if (current_state == PX_WPP_SENDLIST) { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); } else if (current_state == PX_WPP_SENDLIST_SENDWPS) { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); + else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); } } } @@ -717,7 +728,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //we we're target but already communicating with someone else if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); } } break; @@ -735,8 +746,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { if (wpc.count > 0) { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); current_state = PX_WPP_GETLIST; protocol_current_wp_id = 0; @@ -744,7 +755,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* protocol_current_partner_compid = msg->compid; protocol_current_count = wpc.count; - printf("clearing receive buffer and readying for receiving waypoints\n"); + qDebug("clearing receive buffer and readying for receiving waypoints\n"); while(waypoints_receive_buffer->size() > 0) { delete waypoints_receive_buffer->back(); @@ -755,14 +766,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* } else { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); } } else { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); + else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); + else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); } } break; @@ -780,9 +791,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) { - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); current_state = PX_WPP_GETLIST_GETWPS; protocol_current_wp_id = wp.seq + 1; @@ -792,7 +803,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) { - if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); + if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); @@ -813,7 +824,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* if (waypoints->at(i)->current == 1) { current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); yawReached = false; posReached = false; send_waypoint_current(current_active_wp_id); @@ -844,36 +855,36 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { //we're done receiving waypoints, answer with ack. send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); + qDebug("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); } if (verbose) { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } + if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } else if (current_state == PX_WPP_GETLIST) { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); } else if (current_state == PX_WPP_GETLIST_GETWPS) { - if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); + else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); } } } else { - //we we're target but already communicating with someone else + // We're target but already communicating with someone else if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); } else if(wp.target_system == systemid && wp.target_component == compid) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } } break; @@ -888,7 +899,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { protocol_timestamp_lastaction = now; - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); while(waypoints->size() > 0) { delete waypoints->back(); @@ -898,14 +909,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* } else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); + if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); } break; } default: { - if (debug) printf("Waypoint: received message of unknown type\n"); + if (debug) qDebug("Waypoint: received message of unknown type\n"); break; } } @@ -920,7 +931,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* if (timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq); send_waypoint_reached(cur_wp->seq); timestamp_firstinside_orbit = now; } @@ -950,7 +961,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* waypoints->at(current_active_wp_id)->current = true; posReached = false; //yawReached = false; - if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); + if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id); } } } diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.h b/src/comm/MAVLinkSimulationWaypointPlanner.h index 05176d3a6ffb43ef724042761a4fc653ce15db14..12e2b066e3b8820a47f140f2855ff4896ee02e0a 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.h +++ b/src/comm/MAVLinkSimulationWaypointPlanner.h @@ -24,6 +24,7 @@ public: explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid); signals: + void messageSent(const mavlink_message_t& msg); public slots: void handleMessage(const mavlink_message_t& msg); diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc new file mode 100644 index 0000000000000000000000000000000000000000..a196e45a1bb7b817f35795666027c16e666789d2 --- /dev/null +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -0,0 +1,99 @@ +#include "QGCMAVLinkUASFactory.h" +#include "UASManager.h" + +QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) : + QObject(parent) +{ +} + +UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent) +{ + QPointer p; + + if (parent != NULL) + { + p = parent; + } + else + { + p = mavlink; + } + + UASInterface* uas; + + switch (heartbeat->autopilot) + { + case MAV_AUTOPILOT_GENERIC: + { + UAS* mav = new UAS(mavlink, sysid); + // Set the system type + mav->setSystemType((int)heartbeat->type); + // Connect this robot to the UAS object + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } + break; + case MAV_AUTOPILOT_PIXHAWK: + { + PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid); + // Set the system type + mav->setSystemType((int)heartbeat->type); + // 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 + // packets never reach their goal) + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } + break; + case MAV_AUTOPILOT_SLUGS: + { + SlugsMAV* mav = new SlugsMAV(mavlink, sysid); + // Set the system type + mav->setSystemType((int)heartbeat->type); + // 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 + // packets never reach their goal) + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } + break; + case MAV_AUTOPILOT_ARDUPILOTMEGA: + { + ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid); + // Set the system type + mav->setSystemType((int)heartbeat->type); + // 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 + // packets never reach their goal) + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } + break; + default: + { + UAS* mav = new UAS(mavlink, sysid); + mav->setSystemType((int)heartbeat->type); + // 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 + // packets never reach their goal) + connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); + uas = mav; + } + break; + } + + // Set the autopilot type + uas->setAutopilotType((int)heartbeat->autopilot); + + // Make UAS aware that this link can be used to communicate with the actual robot + uas->addLink(link); + + // Now add UAS to "official" list, which makes the whole application aware of it + UASManager::instance()->addUAS(uas); + + return uas; +} diff --git a/src/uas/QGCMAVLinkUASFactory.h b/src/uas/QGCMAVLinkUASFactory.h new file mode 100644 index 0000000000000000000000000000000000000000..992ea20920bf6bb298b764c8700c847ac3f70980 --- /dev/null +++ b/src/uas/QGCMAVLinkUASFactory.h @@ -0,0 +1,32 @@ +#ifndef QGCMAVLINKUASFACTORY_H +#define QGCMAVLINKUASFACTORY_H + +#include + +#include "QGCMAVLink.h" +#include "MAVLinkProtocol.h" +#include "UASInterface.h" +#include "LinkInterface.h" + +// INCLUDE ALL MAV/UAS CLASSES USING MAVLINK +#include "UAS.h" +#include "SlugsMAV.h" +#include "PxQuadMAV.h" +#include "ArduPilotMegaMAV.h" + +class QGCMAVLinkUASFactory : public QObject +{ + Q_OBJECT +public: + explicit QGCMAVLinkUASFactory(QObject *parent = 0); + + /** @brief Create a new UAS object using MAVLink as protocol */ + static UASInterface* createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent=NULL); + +signals: + +public slots: + +}; + +#endif // QGCMAVLINKUASFACTORY_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8a7ddd5e3078eaa0e4d24f0a830ba9793bddbe78..3ed41e635198bcdc12e0fe03ce73a134e79deadf 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -149,6 +149,18 @@ void UAS::setSelected() UASManager::instance()->setActiveUAS(this); } +void UAS::receiveMessageNamedValue(const mavlink_message_t& message) +{ + if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) + { + + } + else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) + { + + } +} + void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { if (!link) return; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 4d16299d39765395312de6fd4a02e3c68de10574..74de151d6b971df402e38c2eab8c7869997e2b63 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -172,10 +172,12 @@ public: UASWaypointManager &getWaypointManager(void) { return waypointManager; } int getSystemType(); int getAutopilotType() {return autopilot;} - void setAutopilotType(int apType) { autopilot = apType;} public slots: - + /** @brief Set the autopilot type */ + void setAutopilotType(int apType) { autopilot = apType; } + /** @brief Set the type of airframe */ + void setSystemType(int systemType) { type = systemType; } /** @brief Set a new name **/ void setUASName(const QString& name); /** @brief Executes an action **/ @@ -307,6 +309,10 @@ signals: void writeSettings(); /** @brief Read settings from disk */ void readSettings(); + + // MESSAGE RECEPTION + /** @brief Receive a named value message */ + void receiveMessageNamedValue(const mavlink_message_t& message); }; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index c63cb532333a52ad09b311f182b603430b188c7d..91b56f80c17db618573e86838400ab08111f8a93 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -123,6 +123,10 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui qDebug() << "No waypoints on UAS " << systemId; } } + else + { + qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId); + } } void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp) @@ -165,9 +169,13 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ } else { - //MainWindow::instance()->showStatusMessage(tr("Waypoint ID mismatch, rejecting waypoint")); + emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint")); } } + else + { + qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId); + } } void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa) @@ -210,6 +218,10 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m //TODO: Error message or something } } + else + { + qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId); + } } void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr) diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 48c99b065e9a1e2974c70f2496a36f36589b3327..1159ff38310bc86a7ce8878588145c063cb6306c 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1155,6 +1155,10 @@ void MainWindow::connectCommonActions() // Custom widget actions connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget())); + + // Audio output + ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); + connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); } void MainWindow::connectPxActions() @@ -1388,14 +1392,14 @@ void MainWindow::UASCreated(UASInterface* uas) } break; - loadOperatorView(); + loadOperatorView(); } // Change the view only if this is the first UAS // If this is the first connected UAS, it is both created as well as // the currently active UAS - if (UASManager::instance()->getActiveUAS() == uas) + if (UASManager::instance()->getUASList().size() == 1) { //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; @@ -1410,8 +1414,7 @@ void MainWindow::UASCreated(UASInterface* uas) if (settings.contains(getWindowStateKey())) { restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); - } - + } } else { diff --git a/src/ui/SlugsHilSim.cc b/src/ui/SlugsHilSim.cc index 58849e56321fb3816d4a3143ed222d87b00b1413..3530c8154fa346ff9d291b9d2abc97374c27632f 100644 --- a/src/ui/SlugsHilSim.cc +++ b/src/ui/SlugsHilSim.cc @@ -252,7 +252,8 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne } -void SlugsHilSim::linkSelected(int cbIndex){ +void SlugsHilSim::linkSelected(int cbIndex) +{ #ifdef MAVLINK_ENABLED_SLUGS // HIL code to go here... //hilLink = linksAvailable @@ -260,6 +261,8 @@ void SlugsHilSim::linkSelected(int cbIndex){ hilLink =linksAvailable.value(cbIndex); +#else + Q_UNUSED(cbIndex) #endif } diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 8843f642962171aa4439002e160078ef26c8ac1d..c387583c0b6ae6f7d2112d0037d6e05f04dfbbc6 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -137,6 +137,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : { m_ui->positionLabel->hide(); } + + setSystemType(uas, uas->getSystemType()); } UASView::~UASView()