diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9e1ddc6d1c09dc10a56c3ab56280042d3dc69027..c9444d4f4ec7f62ed8f48c227a650d2b0bc9bb50 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -210,7 +210,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/map/QGCMapToolBar.ui \ src/ui/QGCMAVLinkInspector.ui \ src/ui/WaypointViewOnlyView.ui \ - src/ui/WaypointEditableView.ui + src/ui/WaypointEditableView.ui \ + src/ui/UnconnectedUASInfoWidget.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -325,7 +326,8 @@ HEADERS += src/MG.h \ src/ui/MAVLinkDecoder.h \ src/ui/WaypointViewOnlyView.h \ src/ui/WaypointViewOnlyView.h \ - src/ui/WaypointEditableView.h + src/ui/WaypointEditableView.h \ + src/ui/UnconnectedUASInfoWidget.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -451,7 +453,8 @@ SOURCES += src/main.cc \ src/ui/QGCMAVLinkInspector.cc \ src/ui/MAVLinkDecoder.cc \ src/ui/WaypointViewOnlyView.cc \ - src/ui/WaypointEditableView.cc + src/ui/WaypointEditableView.cc \ + src/ui/UnconnectedUASInfoWidget.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 509ef391a9e1df0d6cce0982250a4a70d75103ba..44c0f6f134afb7337be5c03b0b91bc9c87e00b21 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -28,59 +28,59 @@ #include "SerialLink.h" UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), -uasId(id), -startTime(QGC::groundTimeMilliseconds()), -commStatus(COMM_DISCONNECTED), -name(""), -autopilot(-1), -links(new QList()), -unknownPackets(), -mavlink(protocol), -waypointManager(*this), -thrustSum(0), -thrustMax(10), -startVoltage(0), -warnVoltage(9.5f), -warnLevelPercent(20.0f), -currentVoltage(12.0f), -lpVoltage(12.0f), -batteryRemainingEstimateEnabled(true), -mode(-1), -status(-1), -navMode(-1), -onboardTimeOffset(0), -controlRollManual(true), -controlPitchManual(true), -controlYawManual(true), -controlThrustManual(true), -manualRollAngle(0), -manualPitchAngle(0), -manualYawAngle(0), -manualThrust(0), -receiveDropRate(0), -sendDropRate(0), -lowBattAlarm(false), -positionLock(false), -localX(0.0), -localY(0.0), -localZ(0.0), -latitude(0.0), -longitude(0.0), -altitude(0.0), -roll(0.0), -pitch(0.0), -yaw(0.0), -statusTimeout(new QTimer(this)), -paramsOnceRequested(false), -airframe(QGC_AIRFRAME_EASYSTAR), -attitudeKnown(false), -paramManager(NULL), -attitudeStamped(false), -lastAttitude(0), -simulation(new QGCFlightGearLink(this)), -isLocalPositionKnown(false), -isGlobalPositionKnown(false), -systemIsArmed(false) + uasId(id), + startTime(QGC::groundTimeMilliseconds()), + commStatus(COMM_DISCONNECTED), + name(""), + autopilot(-1), + links(new QList()), + unknownPackets(), + mavlink(protocol), + waypointManager(this), + thrustSum(0), + thrustMax(10), + startVoltage(0), + warnVoltage(9.5f), + warnLevelPercent(20.0f), + currentVoltage(12.0f), + lpVoltage(12.0f), + batteryRemainingEstimateEnabled(true), + mode(-1), + status(-1), + navMode(-1), + onboardTimeOffset(0), + controlRollManual(true), + controlPitchManual(true), + controlYawManual(true), + controlThrustManual(true), + manualRollAngle(0), + manualPitchAngle(0), + manualYawAngle(0), + manualThrust(0), + receiveDropRate(0), + sendDropRate(0), + lowBattAlarm(false), + positionLock(false), + localX(0.0), + localY(0.0), + localZ(0.0), + latitude(0.0), + longitude(0.0), + altitude(0.0), + roll(0.0), + pitch(0.0), + yaw(0.0), + statusTimeout(new QTimer(this)), + paramsOnceRequested(false), + airframe(QGC_AIRFRAME_EASYSTAR), + attitudeKnown(false), + paramManager(NULL), + attitudeStamped(false), + lastAttitude(0), + simulation(new QGCFlightGearLink(this)), + isLocalPositionKnown(false), + isGlobalPositionKnown(false), + systemIsArmed(false) { color = UASInterface::getNextColor(); setBatterySpecs(QString("9V,9.5V,12.6V")); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 77c0c90fe69e57d3ca5f204bed61691797ad366d..02841e7c61b6629d84732b5dcba5fab72d1371da 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -34,10 +34,10 @@ This file is part of the QGROUNDCONTROL project #include "mavlink_types.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout -#define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages -#define PROTOCOL_MAX_RETRIES 3 ///< maximum number of send retries (after timeout) +#define PROTOCOL_DELAY_MS 20 ///< minimum delay between sent messages +#define PROTOCOL_MAX_RETRIES 5 ///< maximum number of send retries (after timeout) -UASWaypointManager::UASWaypointManager(UAS &_uas) +UASWaypointManager::UASWaypointManager(UAS* _uas) : uas(_uas), current_retries(0), current_wp_id(0), @@ -48,11 +48,16 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) protocol_timer(this), currentWaypointEditable(NULL) { - connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); if (uas) { - connect(&uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); - connect(&uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + uasid = uas->getUASID(); + connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + } + else + { + uasid = 0; } } @@ -112,8 +117,6 @@ void UASWaypointManager::handleGlobalPositionChanged(UASInterface* mav, double l void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count) { - if (uas) - { if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; @@ -140,14 +143,9 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui 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); } } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; - -} void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_mission_item_t *wp) { - if (uas) - { if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; @@ -195,9 +193,6 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ 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); } } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; - -} void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_mission_ack_t *wpa) { @@ -219,66 +214,55 @@ void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavli void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_mission_request_t *wpr) { - if (uas) - { - if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)))) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_retries = PROTOCOL_MAX_RETRIES; + if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)))) { + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; - if (wpr->seq < waypoint_buffer.count()) { - current_state = WP_SENDLIST_SENDWPS; - current_wp_id = wpr->seq; - sendWaypoint(current_wp_id); - } else { - //TODO: Error message or something - } + if (wpr->seq < waypoint_buffer.count()) { + current_state = WP_SENDLIST_SENDWPS; + current_wp_id = wpr->seq; + sendWaypoint(current_wp_id); } 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); + //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); } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; - } void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr) { - if (uas) - { - if (systemId == uas.getUASID()) { - emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq)); - } + if (!uas) return; + if (systemId == uasid) { + emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq)); } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; } void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc) { - if (uas) - { - if (systemId == uas.getUASID()) { - // FIXME Petri - if (current_state == WP_SETCURRENT) { - protocol_timer.stop(); - current_state = WP_IDLE; + if (!uas) return; + if (systemId == uasid) { + // FIXME Petri + if (current_state == WP_SETCURRENT) { + protocol_timer.stop(); + current_state = WP_IDLE; - // update the local main storage - if (wpc->seq < waypointsViewOnly.size()) { - for(int i = 0; i < waypointsViewOnly.size(); i++) { - if (waypointsViewOnly[i]->getId() == wpc->seq) { - waypointsViewOnly[i]->setCurrent(true); - //currentWaypointEditable = waypoints[i]; - } else { - waypointsViewOnly[i]->setCurrent(false); - } + // update the local main storage + if (wpc->seq < waypointsViewOnly.size()) { + for(int i = 0; i < waypointsViewOnly.size(); i++) { + if (waypointsViewOnly[i]->getId() == wpc->seq) { + waypointsViewOnly[i]->setCurrent(true); + //currentWaypointEditable = waypoints[i]; + } else { + waypointsViewOnly[i]->setCurrent(false); } } } - emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); - //emit update to UI widgets - emit currentWaypointChanged(wpc->seq); } + emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); + //emit update to UI widgets + emit currentWaypointChanged(wpc->seq); } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; } void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp) @@ -286,50 +270,46 @@ void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp) // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId(); // If only one waypoint was changed, emit only WP signal if (wp != NULL) { - if (uas) emit waypointEditableChanged(uas.getUASID(), wp); + emit waypointEditableChanged(uasid, wp); } else { emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } void UASWaypointManager::notifyOfChangeViewOnly(Waypoint* wp) { if (wp != NULL) { - if (uas) emit waypointViewOnlyChanged(uas.getUASID(), wp); + emit waypointViewOnlyChanged(uasid, wp); } else { emit waypointViewOnlyListChanged(); - if (uas) emit waypointViewOnlyListChanged(uas.getUASID()); + emit waypointViewOnlyListChanged(uasid); } } int UASWaypointManager::setCurrentWaypoint(quint16 seq) { - if (uas) - { - if (seq < waypointsViewOnly.size()) { - if(current_state == WP_IDLE) { + if (seq < waypointsViewOnly.size()) { + if(current_state == WP_IDLE) { - //send change to UAS - important to note: if the transmission fails, we have inconsistencies - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_retries = PROTOCOL_MAX_RETRIES; + //send change to UAS - important to note: if the transmission fails, we have inconsistencies + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; - current_state = WP_SETCURRENT; - current_wp_id = seq; - current_partner_systemid = uas.getUASID(); - current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; + current_state = WP_SETCURRENT; + current_wp_id = seq; + current_partner_systemid = uasid; + current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; - sendWaypointSetCurrent(current_wp_id); + sendWaypointSetCurrent(current_wp_id); - //emit waypointListChanged(); + //emit waypointListChanged(); - return 0; - } + return 0; } - return -1; } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; + return -1; } int UASWaypointManager::setCurrentEditable(quint16 seq) @@ -355,12 +335,12 @@ int UASWaypointManager::setCurrentEditable(quint16 seq) void UASWaypointManager::addWaypointViewOnly(Waypoint *wp) { if (wp) - { + { waypointsViewOnly.insert(waypointsViewOnly.size(), wp); connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeViewOnly(Waypoint*))); emit waypointViewOnlyListChanged(); - if (uas) emit waypointViewOnlyListChanged(uas.getUASID()); + emit waypointViewOnlyListChanged(uasid); } } @@ -384,7 +364,7 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } @@ -404,7 +384,7 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); return wp; } @@ -423,7 +403,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq) } emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); return 0; } return -1; @@ -450,7 +430,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq) waypointsEditable[new_seq] = t; emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } @@ -516,30 +496,22 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) emit loadWPFile(); emit waypointEditableListChanged(); - if (uas) emit waypointEditableListChanged(uas.getUASID()); - + emit waypointEditableListChanged(uasid); } void UASWaypointManager::clearWaypointList() { - if (uas) + if(current_state == WP_IDLE) { - if(current_state == WP_IDLE) - { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_retries = PROTOCOL_MAX_RETRIES; + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; - current_state = WP_CLEARLIST; - current_wp_id = 0; - current_partner_systemid = uas.getUASID(); - current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; + current_state = WP_CLEARLIST; + current_wp_id = 0; + current_partner_systemid = uasid; + current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; - sendWaypointClearAll(); - } - } - else - { - //todo: if offline, clear edit list + sendWaypointClearAll(); } } @@ -761,8 +733,6 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) */ void UASWaypointManager::readWaypoints(bool readToEdit) { - if (uas) - { read_to_edit = readToEdit; emit readGlobalWPFromUAS(true); if(current_state == WP_IDLE) { @@ -786,20 +756,16 @@ void UASWaypointManager::readWaypoints(bool readToEdit) current_state = WP_GETLIST; current_wp_id = 0; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; sendWaypointRequestList(); } } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::writeWaypoints() { - if (uas) - { if (current_state == WP_IDLE) { // Send clear all if count == 0 if (waypointsEditable.count() > 0) { @@ -809,7 +775,7 @@ void UASWaypointManager::writeWaypoints() current_count = waypointsEditable.count(); current_state = WP_SENDLIST; current_wp_id = 0; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; //clear local buffer @@ -859,166 +825,144 @@ void UASWaypointManager::writeWaypoints() sendWaypointClearAll(); } else { //we're in another transaction, ignore command - qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command"; + qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command"; } } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::sendWaypointClearAll() { - if (uas) - { + if (!uas) return; mavlink_message_t message; mavlink_mission_clear_all_t wpca; - wpca.target_system = uas.getUASID(); + wpca.target_system = uasid; wpca.target_component = MAV_COMP_ID_MISSIONPLANNER; emit updateStatusString(QString("Clearing waypoint list...")); - mavlink_msg_mission_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); + mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint clear all to ID " << wpca.target_system; } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) { - if (uas) - { + if (!uas) return; mavlink_message_t message; mavlink_mission_set_current_t wpsc; - wpsc.target_system = uas.getUASID(); + wpsc.target_system = uasid; wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER; wpsc.seq = seq; emit updateStatusString(QString("Updating target waypoint...")); - mavlink_msg_mission_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); + mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system; } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::sendWaypointCount() { - if (uas) - { + if (!uas) return; mavlink_message_t message; mavlink_mission_count_t wpc; - wpc.target_system = uas.getUASID(); + wpc.target_system = uasid; wpc.target_component = MAV_COMP_ID_MISSIONPLANNER; wpc.count = current_count; // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; emit updateStatusString(QString("Starting to transmit waypoints...")); - mavlink_msg_mission_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); + mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::sendWaypointRequestList() { - if (uas) - { + if (!uas) return; mavlink_message_t message; mavlink_mission_request_list_t wprl; - wprl.target_system = uas.getUASID(); + wprl.target_system = uasid; wprl.target_component = MAV_COMP_ID_MISSIONPLANNER; emit updateStatusString(QString("Requesting waypoint list...")); - mavlink_msg_mission_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); + mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint list request to ID " << wprl.target_system; -} -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; + } void UASWaypointManager::sendWaypointRequest(quint16 seq) { - if (uas) - { + if (!uas) return; mavlink_message_t message; mavlink_mission_request_t wpr; - wpr.target_system = uas.getUASID(); + wpr.target_system = uasid; wpr.target_component = MAV_COMP_ID_MISSIONPLANNER; wpr.seq = seq; emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count)); - mavlink_msg_mission_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); + mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system; } -else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; -} void UASWaypointManager::sendWaypoint(quint16 seq) { - if (uas) - { - mavlink_message_t message; - // // qDebug() <<" WP Buffer count: "<target_system = uas.getUASID(); - wp->target_component = MAV_COMP_ID_MISSIONPLANNER; + wp = waypoint_buffer.at(seq); + wp->target_system = uasid; + wp->target_component = MAV_COMP_ID_MISSIONPLANNER; - emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); + emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); - // // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<seq << ") to ID " << wp->target_system<<" WP Buffer count: "<getSystemId(), uas.mavlink->getComponentId(), &message, wp); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); - } + mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; } void UASWaypointManager::sendWaypointAck(quint8 type) { - if (uas) - { - mavlink_message_t message; - mavlink_mission_ack_t wpa; + if (!uas) return; + mavlink_message_t message; + mavlink_mission_ack_t wpa; - wpa.target_system = uas.getUASID(); - wpa.target_component = MAV_COMP_ID_MISSIONPLANNER; - wpa.type = type; + wpa.target_system = uasid; + wpa.target_component = MAV_COMP_ID_MISSIONPLANNER; + wpa.type = type; - mavlink_msg_mission_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa); - uas.sendMessage(message); - MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); - } - else qDebug() << "Dummy UASWaypointManager: Cannot execute this function."; + mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa); + if (uas) uas->sendMessage(message); + QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); // // qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system; } diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 6f35996cb6a4ba0e207e52fc83d15d2f2b0350c7..d195fcd4976f5dc0c791565f3478e69b51fc3767 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -64,7 +64,7 @@ private: }; ///< The possible states for the waypoint protocol public: - UASWaypointManager(UAS&); ///< Standard constructor. + UASWaypointManager(UAS* uas=NULL); ///< Standard constructor /** @name Received message handlers */ /*@{*/ @@ -109,7 +109,7 @@ public: int getLocalFrameCount(); ///< Get the count of local waypoints in the list /*@}*/ - UAS& getUAS() { + UAS* getUAS() { return this->uas; ///< Returns the owning UAS } @@ -157,7 +157,7 @@ signals: void readGlobalWPFromUAS(bool value); ///< emits signal when finish to read Global WP from UAS private: - UAS &uas; ///< Reference to the corresponding UAS + UAS* uas; ///< Reference to the corresponding UAS quint32 current_retries; ///< The current number of retries left quint16 current_wp_id; ///< The last used waypoint ID in the current protocol transaction quint16 current_count; ///< The number of waypoints in the current protocol transaction @@ -171,6 +171,8 @@ private: Waypoint* currentWaypointEditable; ///< The currently used waypoint QVector waypoint_buffer; ///< buffer for waypoints during communication QTimer protocol_timer; ///< Timer to catch timeouts + bool standalone; ///< If standalone is set, do not write to UAS + quint16 uasid; }; #endif // UASWAYPOINTMANAGER_H diff --git a/src/ui/QGCWaypointListMulti.cc b/src/ui/QGCWaypointListMulti.cc index 5b8c68fc489dc0672f6866f2ccc13512c479c21f..2bf3462c84956102c001e0483590a39d93c53e03 100644 --- a/src/ui/QGCWaypointListMulti.cc +++ b/src/ui/QGCWaypointListMulti.cc @@ -4,19 +4,17 @@ QGCWaypointListMulti::QGCWaypointListMulti(QWidget *parent) : QWidget(parent), - ui(new Ui::QGCWaypointListMulti) + ui(new Ui::QGCWaypointListMulti), + offline_uas_id(0) { ui->setupUi(this); setMinimumSize(600, 80); connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(systemCreated(UASInterface*))); connect(UASManager::instance(), SIGNAL(activeUASSet(int)), this, SLOT(systemSetActive(int))); - WaypointList* list = new WaypointList(ui->stackedWidget, uas); - lists.insert(uas->getUASID(), list); + WaypointList* list = new WaypointList(ui->stackedWidget, NULL); + lists.insert(offline_uas_id, list); ui->stackedWidget->addWidget(list); - // Ensure widget is deleted when system is deleted - connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(systemDeleted(QObject*))); - } void QGCWaypointListMulti::systemDeleted(QObject* uas) diff --git a/src/ui/QGCWaypointListMulti.h b/src/ui/QGCWaypointListMulti.h index 13bfb4ecfa957c944735dc135184c074cc6b0aba..392d79ec1ef197b4bd00c913c2ab14126a02c9db 100644 --- a/src/ui/QGCWaypointListMulti.h +++ b/src/ui/QGCWaypointListMulti.h @@ -26,6 +26,7 @@ public slots: void systemSetActive(int uas); protected: + quint16 offline_uas_id; void changeEvent(QEvent *e); QMap lists; diff --git a/src/ui/UnconnectedUASInfoWidget.cc b/src/ui/UnconnectedUASInfoWidget.cc new file mode 100644 index 0000000000000000000000000000000000000000..1781b63ee95e1a9e7487a4169d45e1d4d43f9b0b --- /dev/null +++ b/src/ui/UnconnectedUASInfoWidget.cc @@ -0,0 +1,14 @@ +#include "UnconnectedUASInfoWidget.h" +#include "ui_UnconnectedUASInfoWidget.h" + +UnconnectedUASInfoWidget::UnconnectedUASInfoWidget(QWidget *parent) : + QGroupBox(parent), + ui(new Ui::UnconnectedUASInfoWidget) +{ + ui->setupUi(this); +} + +UnconnectedUASInfoWidget::~UnconnectedUASInfoWidget() +{ + delete ui; +} diff --git a/src/ui/UnconnectedUASInfoWidget.h b/src/ui/UnconnectedUASInfoWidget.h new file mode 100644 index 0000000000000000000000000000000000000000..ea349f85790f126ad526313e9c490fc63af3dbe6 --- /dev/null +++ b/src/ui/UnconnectedUASInfoWidget.h @@ -0,0 +1,22 @@ +#ifndef UNCONNECTEDUASINFOWIDGET_H +#define UNCONNECTEDUASINFOWIDGET_H + +#include + +namespace Ui { + class UnconnectedUASInfoWidget; +} + +class UnconnectedUASInfoWidget : public QGroupBox +{ + Q_OBJECT + +public: + explicit UnconnectedUASInfoWidget(QWidget *parent = 0); + ~UnconnectedUASInfoWidget(); + +private: + Ui::UnconnectedUASInfoWidget *ui; +}; + +#endif // UNCONNECTEDUASINFOWIDGET_H diff --git a/src/ui/UnconnectedUASInfoWidget.ui b/src/ui/UnconnectedUASInfoWidget.ui new file mode 100644 index 0000000000000000000000000000000000000000..880f715360154ec0221f16543961382df9aeaf84 --- /dev/null +++ b/src/ui/UnconnectedUASInfoWidget.ui @@ -0,0 +1,56 @@ + + + UnconnectedUASInfoWidget + + + + 0 + 0 + 279 + 114 + + + + + + + + + + + + + 0 + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + QTextEdit::AutoAll + + + false + + + true + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> +<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> +<tr> +<td style="border: none;"> +<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:20pt; font-weight:600; color:#b5b5b5;">NO UAS</span></p></td></tr></table></body></html> + + + + + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 0e1d983d96d9300859b4a77088c92f3b906b42e5..09db4ec4e11f28537390caf50073a141d8184e2f 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -49,6 +49,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : mavYaw(0.0), m_ui(new Ui::WaypointList) { + m_ui->setupUi(this); //EDIT TAB @@ -70,6 +71,9 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : // SEND WAYPOINTS connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit())); + // DELETE ALL WAYPOINTS + connect(m_ui->clearWPListButton, SIGNAL(clicked()), this, SLOT(clearWPWidget())); + // REQUEST WAYPOINTS connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read())); @@ -95,9 +99,25 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED if (uas) { - setUAS(uas); + qDebug() << "setUAS" ; + WPM = uas->getWaypointManager(); + } + else + { + qDebug() << "setUAS failed" ; + // Hide buttons, which don't make sense without valid UAS + m_ui->positionAddButton->hide(); + m_ui->transmitButton->hide(); + m_ui->readButton->hide(); + //FIXME: The whole "Onboard Waypoints"-tab should be hidden, instead of "refresh" button + m_ui->refreshButton->hide(); + UnconnectedUASInfoWidget* inf = new UnconnectedUASInfoWidget(this); + viewOnlyListLayout->insertWidget(0, inf); + WPM = new UASWaypointManager(NULL); } + setUAS(uas); + // STATUS LABEL updateStatusLabel(""); @@ -134,47 +154,46 @@ void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, void WaypointList::setUAS(UASInterface* uas) { - if (this->uas == NULL && uas != NULL) + //if (this->uas == NULL && uas != NULL) + if (this->uas == NULL) { this->uas = uas; connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(uas->getWaypointManager(), SIGNAL(waypointEditableListChanged(void)), this, SLOT(waypointEditableListChanged(void))); - connect(uas->getWaypointManager(), SIGNAL(waypointEditableChanged(int,Waypoint*)), this, SLOT(updateWaypointEditable(int,Waypoint*))); - connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(waypointViewOnlyListChanged(void))); - connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(updateWaypointViewOnly(int,Waypoint*))); - connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointViewOnlyChanged(quint16))); - //connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); - //connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); + connect(WPM, SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); + connect(WPM, SIGNAL(waypointEditableListChanged(void)), this, SLOT(waypointEditableListChanged(void))); + connect(WPM, SIGNAL(waypointEditableChanged(int,Waypoint*)), this, SLOT(updateWaypointEditable(int,Waypoint*))); + connect(WPM, SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(waypointViewOnlyListChanged(void))); + connect(WPM, SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(updateWaypointViewOnly(int,Waypoint*))); + connect(WPM, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointViewOnlyChanged(quint16))); + //connect(WPM,SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); + //connect(WPM,SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); } } void WaypointList::saveWaypoints() { - if (uas) - { + QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - uas->getWaypointManager()->saveWaypoints(fileName); - } + WPM->saveWaypoints(fileName); + } void WaypointList::loadWaypoints() { - if (uas) - { + QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - uas->getWaypointManager()->loadWaypoints(fileName); - } + WPM->loadWaypoints(fileName); + } void WaypointList::transmit() { if (uas) { - uas->getWaypointManager()->writeWaypoints(); + WPM->writeWaypoints(); } } @@ -182,7 +201,7 @@ void WaypointList::read() { if (uas) { - uas->getWaypointManager()->readWaypoints(true); + WPM->readWaypoints(true); } } @@ -190,38 +209,14 @@ void WaypointList::refresh() { if (uas) { - uas->getWaypointManager()->readWaypoints(false); + WPM->readWaypoints(false); } } void WaypointList::addEditable() { - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); - Waypoint *wp; - if (waypoints.size() > 0) - { - // Create waypoint with last frame - Waypoint *last = waypoints.at(waypoints.size()-1); - wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), - last->getAutoContinue(), false, last->getFrame(), last->getAction()); - uas->getWaypointManager()->addWaypointEditable(wp); - } - else - { - // Create first waypoint at current MAV position - addCurrentPositionWaypoint(); - } - } -} -/* -void WaypointList::addViewOnly() -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); + const QVector &waypoints = WPM->getWaypointEditableList(); Waypoint *wp; if (waypoints.size() > 0) { @@ -229,22 +224,32 @@ void WaypointList::addViewOnly() Waypoint *last = waypoints.at(waypoints.size()-1); wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), last->getAutoContinue(), false, last->getFrame(), last->getAction()); - uas->getWaypointManager()->addWaypointEditable(wp); + WPM->addWaypointEditable(wp); } else - { - // Create first waypoint at current MAV position - addCurrentPositionWaypoint(); + { + if (uas) + { + // Create first waypoint at current MAV position + addCurrentPositionWaypoint(); + } + else + { + //Since no UAV available, create first default waypoint. + updateStatusLabel(tr("No UAV. Added default LOCAL (NED) waypoint")); + wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); + } } - } + } -*/ + void WaypointList::addCurrentPositionWaypoint() -{ +{ if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + const QVector &waypoints = WPM->getWaypointEditableList(); Waypoint *wp; Waypoint *last = 0; if (waypoints.size() > 0) @@ -265,7 +270,7 @@ void WaypointList::addCurrentPositionWaypoint() } // Create global frame waypoint per default wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); - uas->getWaypointManager()->addWaypointEditable(wp); + WPM->addWaypointEditable(wp); updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); } else if (uas->localPositionKnown()) @@ -279,13 +284,13 @@ void WaypointList::addCurrentPositionWaypoint() } // Create local frame waypoint as second option wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - uas->getWaypointManager()->addWaypointEditable(wp); + WPM->addWaypointEditable(wp); updateStatusLabel(tr("Added LOCAL (NED) waypoint")); } else { // Do nothing - updateStatusLabel(tr("Not adding waypoint, no position known of MAV known yet.")); + updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet.")); } } } @@ -303,18 +308,15 @@ void WaypointList::changeCurrentWaypoint(quint16 seq) { if (uas) { - uas->getWaypointManager()->setCurrentWaypoint(seq); + WPM->setCurrentWaypoint(seq); } } // Request UASWaypointManager to set the new "current" and make sure all other waypoints are not "current" void WaypointList::currentWaypointEditableChanged(quint16 seq) { - if (uas) - { - uas->getWaypointManager()->setCurrentEditable(seq); - /* - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + WPM->setCurrentEditable(seq); + const QVector &waypoints = WPM->getWaypointEditableList(); if (seq < waypoints.size()) { @@ -332,32 +334,26 @@ void WaypointList::currentWaypointEditableChanged(quint16 seq) } } } - */ - } - } // Update waypointViews to correctly indicate the new current waypoint void WaypointList::currentWaypointViewOnlyChanged(quint16 seq) { - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); + const QVector &waypoints = WPM->getWaypointViewOnlyList(); - if (seq < waypoints.size()) + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value(); + WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value(); - if (waypoints[i]->getId() == seq) - { - widget->setCurrent(true); - } - else - { - widget->setCurrent(false); - } + if (waypoints[i]->getId() == seq) + { + widget->setCurrent(true); + } + else + { + widget->setCurrent(false); } } } @@ -379,263 +375,149 @@ void WaypointList::updateWaypointViewOnly(int uas, Waypoint* wp) void WaypointList::waypointViewOnlyListChanged() { - if (uas) { - // Prevent updates to prevent visual flicker - this->setUpdatesEnabled(false); - const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); - - if (!wpViewOnlyViews.empty()) { - QMapIterator viewIt(wpViewOnlyViews); - viewIt.toFront(); - while(viewIt.hasNext()) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == cur) { - break; - } - } - if (i == waypoints.size()) { - WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value(); - widget->hide(); - viewOnlyListLayout->removeWidget(widget); - wpViewOnlyViews.remove(cur); + // Prevent updates to prevent visual flicker + this->setUpdatesEnabled(false); + const QVector &waypoints = WPM->getWaypointViewOnlyList(); + + if (!wpViewOnlyViews.empty()) { + QMapIterator viewIt(wpViewOnlyViews); + viewIt.toFront(); + while(viewIt.hasNext()) { + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) { + if (waypoints[i] == cur) { + break; } } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { - Waypoint *wp = waypoints[i]; - if (!wpViewOnlyViews.contains(wp)) { - WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this); - wpViewOnlyViews.insert(wp, wpview); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); - viewOnlyListLayout->insertWidget(i, wpview); + if (i == waypoints.size()) { + WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value(); + widget->hide(); + viewOnlyListLayout->removeWidget(widget); + wpViewOnlyViews.remove(cur); } - WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); + } + } - //check if ordering has changed - if(viewOnlyListLayout->itemAt(i)->widget() != wpv) { - viewOnlyListLayout->removeWidget(wpv); - viewOnlyListLayout->insertWidget(i, wpv); - } + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) { + Waypoint *wp = waypoints[i]; + if (!wpViewOnlyViews.contains(wp)) { + WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this); + wpViewOnlyViews.insert(wp, wpview); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + viewOnlyListLayout->insertWidget(i, wpview); + } + WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view + //check if ordering has changed + if(viewOnlyListLayout->itemAt(i)->widget() != wpv) { + viewOnlyListLayout->removeWidget(wpv); + viewOnlyListLayout->insertWidget(i, wpv); } - this->setUpdatesEnabled(true); - loadFileGlobalWP = false; + + wpv->updateValues(); // update the values of the ui elements in the view } + this->setUpdatesEnabled(true); + loadFileGlobalWP = false; + } void WaypointList::waypointEditableListChanged() { - if (uas) { - // Prevent updates to prevent visual flicker - this->setUpdatesEnabled(false); - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); - - if (!wpEditableViews.empty()) { - QMapIterator viewIt(wpEditableViews); - viewIt.toFront(); - while(viewIt.hasNext()) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == cur) { - break; - } - } - if (i == waypoints.size()) { - WaypointEditableView* widget = wpEditableViews.find(cur).value(); - widget->hide(); - editableListLayout->removeWidget(widget); - wpEditableViews.remove(cur); + // Prevent updates to prevent visual flicker + this->setUpdatesEnabled(false); + const QVector &waypoints = WPM->getWaypointEditableList(); + + if (!wpEditableViews.empty()) { + QMapIterator viewIt(wpEditableViews); + viewIt.toFront(); + while(viewIt.hasNext()) { + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) { + if (waypoints[i] == cur) { + break; } } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { - Waypoint *wp = waypoints[i]; - if (!wpEditableViews.contains(wp)) { - WaypointEditableView* wpview = new WaypointEditableView(wp, this); - wpEditableViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - //connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(currentWaypointEditableChanged(quint16))); - editableListLayout->insertWidget(i, wpview); + if (i == waypoints.size()) { + WaypointEditableView* widget = wpEditableViews.find(cur).value(); + widget->hide(); + editableListLayout->removeWidget(widget); + wpEditableViews.remove(cur); } - WaypointEditableView *wpv = wpEditableViews.value(wp); + } + } - //check if ordering has changed - if(editableListLayout->itemAt(i)->widget() != wpv) { - editableListLayout->removeWidget(wpv); - editableListLayout->insertWidget(i, wpv); - } + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) { + Waypoint *wp = waypoints[i]; + if (!wpEditableViews.contains(wp)) { + WaypointEditableView* wpview = new WaypointEditableView(wp, this); + wpEditableViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + //connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(currentWaypointEditableChanged(quint16))); + editableListLayout->insertWidget(i, wpview); + } + WaypointEditableView *wpv = wpEditableViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view + //check if ordering has changed + if(editableListLayout->itemAt(i)->widget() != wpv) { + editableListLayout->removeWidget(wpv); + editableListLayout->insertWidget(i, wpv); } - this->setUpdatesEnabled(true); - loadFileGlobalWP = false; - } -} -//void WaypointList::waypointEditableListChanged() -//{ -// if (uas) -// { -// // Prevent updates to prevent visual flicker -// this->setUpdatesEnabled(false); -// // Get all waypoints -// const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); - -//// // Store the current state, then check which widgets to update -//// // and which ones to delete -//// QList oldWaypoints = wpEditableViews.keys(); - -//// foreach (Waypoint* wp, waypoints) -//// { -//// WaypointEditableView* wpview; -//// // Create any new waypoint -//// if (!wpEditableViews.contains(wp)) -//// { -//// wpview = new WaypointEditableView(wp, this); -//// wpEditableViews.insert(wp, wpview); -//// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); -//// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); -//// connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); -//// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); -//// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); -//// editableListLayout->addWidget(wpview); -//// } -//// else -//// { -//// // Update existing waypoints -//// wpview = wpEditableViews.value(wp); - -//// } -//// // Mark as updated by removing from old list -//// oldWaypoints.removeAt(oldWaypoints.indexOf(wp)); - -//// wpview->updateValues(); // update the values of the ui elements in the view - -//// } - -//// // The old list now contains all wps to be deleted -//// foreach (Waypoint* wp, oldWaypoints) -//// { -//// // Delete waypoint view and entry in list -//// WaypointEditableView* wpv = wpEditableViews.value(wp); -//// if (wpv) -//// { -//// editableListLayout->removeWidget(wpv); -//// delete wpv; -//// } -//// wpEditableViews.remove(wp); -//// } - -// if (!wpEditableViews.empty()) -// { -// QMapIterator viewIt(wpEditableViews); -// viewIt.toFront(); -// while(viewIt.hasNext()) -// { -// viewIt.next(); -// Waypoint *cur = viewIt.key(); -// int i; -// for (i = 0; i < waypoints.size(); i++) -// { -// if (waypoints[i] == cur) -// { -// break; -// } -// } -// if (i == waypoints.size()) -// { -// WaypointEditableView* widget = wpEditableViews.find(cur).value(); -// if (widget) -// { -// widget->hide(); -// editableListLayout->removeWidget(widget); -// } -// wpEditableViews.remove(cur); -// } -// } -// } + wpv->updateValues(); // update the values of the ui elements in the view + } + this->setUpdatesEnabled(true); + loadFileGlobalWP = false; -// // then add/update the views for each waypoint in the list -// for(int i = 0; i < waypoints.size(); i++) -// { -// Waypoint *wp = waypoints[i]; -// if (!wpEditableViews.contains(wp)) -// { -// WaypointEditableView* wpview = new WaypointEditableView(wp, this); -// wpEditableViews.insert(wp, wpview); -// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); -// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); -// connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); -// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); -// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); -// } -// WaypointEditableView *wpv = wpEditableViews.value(wp); -// wpv->updateValues(); // update the values of the ui elements in the view -// editableListLayout->addWidget(wpv); -// } -// this->setUpdatesEnabled(true); -// } -//// loadFileGlobalWP = false; -//} +} void WaypointList::moveUp(Waypoint* wp) { - if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + const QVector &waypoints = WPM->getWaypointEditableList(); - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == wp) - break; - } + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) { + if (waypoints[i] == wp) + break; + } - // if wp was found and its not the first entry, move it - if (i < waypoints.size() && i > 0) { - uas->getWaypointManager()->moveWaypoint(i, i-1); - } + // if wp was found and its not the first entry, move it + if (i < waypoints.size() && i > 0) { + WPM->moveWaypoint(i, i-1); } } void WaypointList::moveDown(Waypoint* wp) -{ - if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); - - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) { - if (waypoints[i] == wp) - break; - } +{ + const QVector &waypoints = WPM->getWaypointEditableList(); + + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) { + if (waypoints[i] == wp) + break; + } - // if wp was found and its not the last entry, move it - if (i < waypoints.size()-1) { - uas->getWaypointManager()->moveWaypoint(i, i+1); - } + // if wp was found and its not the last entry, move it + if (i < waypoints.size()-1) { + WPM->moveWaypoint(i, i+1); } } void WaypointList::removeWaypoint(Waypoint* wp) -{ - if (uas) { - uas->getWaypointManager()->removeWaypoint(wp->getId()); - } +{ + WPM->removeWaypoint(wp->getId()); } void WaypointList::changeEvent(QEvent *e) @@ -657,7 +539,7 @@ void WaypointList::on_clearWPListButton_clicked() if (uas) { emit clearPathclicked(); - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + const QVector &waypoints = WPM->getWaypointEditableList(); while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); @@ -676,7 +558,7 @@ void WaypointList::on_clearWPListButton_clicked() //{ // if (uas) // { -// const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); +// const QVector &waypoints = WPM->getWaypointEditableList(); // if (waypoints.size() > 0) // { // Waypoint *temp = waypoints.at(indexWP); @@ -706,14 +588,12 @@ void WaypointList::on_clearWPListButton_clicked() //} void WaypointList::clearWPWidget() -{ - if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); +{ + const QVector &waypoints = WPM->getWaypointEditableList(); while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); - } - } + } } //void WaypointList::setIsLoadFileWP() diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 3af991589ba29b71afcd1b619db3ff57d93ad5a2..d773fa76e0a3b5e9fe24cffffb9c2999abb0b659 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -42,6 +42,8 @@ This file is part of the QGROUNDCONTROL project #include "UASInterface.h" #include "WaypointEditableView.h" #include "WaypointViewOnlyView.h" +#include "UnconnectedUASInfoWidget.h" + namespace Ui { @@ -130,6 +132,7 @@ protected: QVBoxLayout* viewOnlyListLayout; QVBoxLayout* editableListLayout; UASInterface* uas; + UASWaypointManager* WPM; double mavX; double mavY; double mavZ; diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index b728e5271dac9c4484a87f3d3859a8a85a88911b..a3cacaec41fe2618c1b50b8e458aedbf41567d50 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -70,8 +70,8 @@ 0 0 - 834 - 325 + 836 + 316 @@ -300,8 +300,8 @@ 0 0 - 834 - 331 + 836 + 316 @@ -367,7 +367,7 @@ - TextLabel + diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.ui b/src/ui/uas/QGCUnconnectedInfoWidget.ui index f7e30998cb458bd9241713b99a0c1964c2cbfb0e..4e57211109a1a18fb99d8aa4eb4959c788e01d63 100644 --- a/src/ui/uas/QGCUnconnectedInfoWidget.ui +++ b/src/ui/uas/QGCUnconnectedInfoWidget.ui @@ -35,19 +35,19 @@ <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> <tr> <td style="border: none;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600;">Unmanned System List</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:6pt; font-weight:600;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">No Systems are connected yet.</span><span style=" font-family:'Ubuntu'; font-size:10pt;"> Please either connect a link or use the simulation function to see QGroundControl in action.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:10pt;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-style:italic;">Click on the simulation button below to simulate a micro air vehicle or on the connect link button to connect a serial port link. A UDP link is already open for connections on port </span><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600; font-style:italic;">14550</span><span style=" font-family:'Ubuntu'; font-size:10pt; font-style:italic;">.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">Communication Link Help:</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:6pt; font-style:italic;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:10pt;">If you encounter communication problems on your link (e.g. no MAV is shown in the list after connecting the link), please check if you receive data on the link using the communication console. Select </span><span style=" font-family:'Ubuntu'; font-size:10pt; font-weight:600;">Tools -&gt; Communication Console</span><span style=" font-family:'Ubuntu'; font-size:10pt;"> to enable it. The console should show incoming traffic and some used bandwidth (e.g. 1.43 kB/s on the indicator).</span></p></td></tr></table></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600;">Unmanned System List</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:6pt; font-weight:600;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt; font-weight:600;">No Systems are connected yet.</span><span style=" font-size:10pt;"> Please either connect a link or use the simulation function to see QGroundControl in action.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt; font-style:italic;">Click on the simulation button below to simulate a micro air vehicle or on the connect link button to connect a serial port link. A UDP link is already open for connections on port </span><span style=" font-size:10pt; font-weight:600; font-style:italic;">14550</span><span style=" font-size:10pt; font-style:italic;">.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt; font-weight:600;">Communication Link Help:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:6pt; font-style:italic;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">If you encounter communication problems on your link (e.g. no MAV is shown in the list after connecting the link), please check if you receive data on the link using the communication console. Select </span><span style=" font-size:10pt; font-weight:600;">Tools -&gt; Communication Console</span><span style=" font-size:10pt;"> to enable it. The console should show incoming traffic and some used bandwidth (e.g. 1.43 kB/s on the indicator).</span></p></td></tr></table></body></html>