diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 1d50c4925acdba2e65bc811a529e35879b2b57ad..5462cf9a3e4d8f5ecf9936f117261b03a1a34971 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -72,7 +72,7 @@ void UASWaypointManager::timeout() protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries--; emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries)); - // // qDebug() << "Timeout, retrying (retries left:" << current_retries << ")"; + if (current_state == WP_GETLIST) { sendWaypointRequestList(); } else if (current_state == WP_GETLIST_GETWPS) { @@ -89,8 +89,6 @@ void UASWaypointManager::timeout() } else { protocol_timer.stop(); - // // qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE"; - emit updateStatusString("Operation timed out."); current_state = WP_IDLE; @@ -136,8 +134,6 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; - // // qDebug() << "got waypoint count (" << count << ") from ID " << systemId; - //Clear the old edit-list before receiving the new one if (read_to_edit == true){ while(waypointsEditable.size()>0) { @@ -156,7 +152,6 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui } else { protocol_timer.stop(); emit updateStatusString("done."); - // // qDebug() << "No waypoints on UAS " << systemId; current_state = WP_IDLE; current_count = 0; current_wp_id = 0; @@ -177,7 +172,6 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ current_retries = PROTOCOL_MAX_RETRIES; if(wp->seq == current_wp_id) { - //// // qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command; Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); addWaypointViewOnly(lwp_vo); @@ -207,10 +201,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ protocol_timer.stop(); emit readGlobalWPFromUAS(false); - //if (currentWaypointEditable) emit currentWaypointChanged(currentWaypointEditable->getId()); emit updateStatusString("done."); - // // qDebug() << "got all waypoints from ID " << systemId; } } else { emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint")); @@ -229,12 +221,10 @@ void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavli current_state = WP_IDLE; readWaypoints(false); //Update "Onboard Waypoints"-tab immidiately after the waypoint list has been sent. emit updateStatusString("done."); - // // qDebug() << "sent all waypoints to ID " << systemId; } else if(current_state == WP_CLEARLIST) { protocol_timer.stop(); current_state = WP_IDLE; emit updateStatusString("done."); - // // qDebug() << "cleared waypoint list of ID " << systemId; } } } @@ -281,7 +271,6 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m 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); } @@ -296,7 +285,6 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp) { - // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId(); // If only one waypoint was changed, emit only WP signal if (wp != NULL) { emit waypointEditableChanged(uasid, wp); @@ -333,8 +321,6 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) sendWaypointSetCurrent(current_wp_id); - //emit waypointListChanged(); - return 0; } } @@ -349,7 +335,6 @@ int UASWaypointManager::setCurrentEditable(quint16 seq) for(int i = 0; i < waypointsEditable.size(); i++) { if (waypointsEditable[i]->getId() == seq) { waypointsEditable[i]->setCurrent(true); - //currentWaypointEditable = waypoints[i]; } else { waypointsEditable[i]->setCurrent(false); } @@ -531,7 +516,6 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) else { emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful.")); - //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),); break; } } @@ -894,11 +878,9 @@ void UASWaypointManager::sendWaypointClearAll() emit updateStatusString(QString("Clearing waypoint list...")); 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; } void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) @@ -914,11 +896,8 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) emit updateStatusString(QString("Updating target waypoint...")); 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; } void UASWaypointManager::sendWaypointCount() @@ -931,15 +910,11 @@ void UASWaypointManager::sendWaypointCount() 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); - //if (uas) uas->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); - - // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; } void UASWaypointManager::sendWaypointRequestList() @@ -954,13 +929,8 @@ void UASWaypointManager::sendWaypointRequestList() emit updateStatusString(QString("Requesting waypoint list...")); 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; - - } void UASWaypointManager::sendWaypointRequest(quint16 seq) @@ -976,18 +946,14 @@ void UASWaypointManager::sendWaypointRequest(quint16 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); - //if (uas) uas->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); - - // // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system; } void UASWaypointManager::sendWaypoint(quint16 seq) { if (!uas) return; mavlink_message_t message; - // // qDebug() <<" WP Buffer count: "<seq).arg(current_count)); - // // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp); - // if (uas) uas->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); } @@ -1020,9 +984,6 @@ void UASWaypointManager::sendWaypointAck(quint8 type) wpa.type = type; 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; }