UASWaypointManager.cc

/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009-2012 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of the waypoint protocol handler
 *
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

#include "UASWaypointManager.h"
#include "UAS.h"
#include "mavlink_types.h"
#include "UASManager.h"
#include "MainWindow.h"

#define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
#define PROTOCOL_DELAY_MS 20        ///< minimum delay between sent messages
#define PROTOCOL_MAX_RETRIES 5      ///< maximum number of send retries (after timeout)
const float UASWaypointManager::defaultAltitudeHomeOffset   = 30.0f;
UASWaypointManager::UASWaypointManager(UAS* _uas)
    : uas(_uas),
      current_retries(0),
      current_wp_id(0),
      current_count(0),
      current_state(WP_IDLE),
      current_partner_systemid(0),
      current_partner_compid(0),
      currentWaypointEditable(NULL),
      protocol_timer(this)
{
    if (uas)
    {
        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;
    }
}

UASWaypointManager::~UASWaypointManager()
{

}

void UASWaypointManager::timeout()
{
    if (current_retries > 0) {
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries--;
        emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries));

        if (current_state == WP_GETLIST) {
            sendWaypointRequestList();
        } else if (current_state == WP_GETLIST_GETWPS) {
            sendWaypointRequest(current_wp_id);
        } else if (current_state == WP_SENDLIST) {
            sendWaypointCount();
        } else if (current_state == WP_SENDLIST_SENDWPS) {
            sendWaypoint(current_wp_id);
        } else if (current_state == WP_CLEARLIST) {
            sendWaypointClearAll();
        } else if (current_state == WP_SETCURRENT) {
            sendWaypointSetCurrent(current_wp_id);
        }
    } else {
        protocol_timer.stop();

        emit updateStatusString("Operation timed out.");

        current_state = WP_IDLE;
        current_count = 0;
        current_wp_id = 0;
        current_partner_systemid = 0;
        current_partner_compid = 0;
    }
}

void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, double y, double z, quint64 time)
{
    Q_UNUSED(mav);
    Q_UNUSED(time);
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU))
    {
        double xdiff = x-currentWaypointEditable->getX();
        double ydiff = y-currentWaypointEditable->getY();
        double zdiff = z-currentWaypointEditable->getZ();
        double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
        emit waypointDistanceChanged(dist);
    }
}

void UASWaypointManager::handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double alt, quint64 time)
{
    Q_UNUSED(mav);
    Q_UNUSED(time);
	Q_UNUSED(alt);
	Q_UNUSED(lon);
	Q_UNUSED(lat);
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL || currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT))
    {
        // TODO FIXME Calculate distance
        double dist = 0;
        emit waypointDistanceChanged(dist);
    }
}

void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
    if (current_state == WP_GETLIST && systemId == current_partner_systemid) {
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

        //Clear the old edit-list before receiving the new one
        if (read_to_edit == true){
            while(waypointsEditable.count()>0) {
                Waypoint *t = waypointsEditable[0];
                waypointsEditable.removeAt(0);
                delete t;
            }
            emit waypointEditableListChanged();
        }

        if (count > 0) {
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
            sendWaypointRequest(current_wp_id);
        } else {
            protocol_timer.stop();
            emit updateStatusString("done.");
            current_state = WP_IDLE;
            current_count = 0;
            current_wp_id = 0;
            current_partner_systemid = 0;
            current_partner_compid = 0;
        }


    } 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_mission_item_t *wp)
{
    if (systemId == current_partner_systemid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) {
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

        if(wp->seq == current_wp_id) {

            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);


            if (read_to_edit == true) {
                Waypoint *lwp_ed = 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);
                addWaypointEditable(lwp_ed, false);
                if (wp->current == 1) currentWaypointEditable = lwp_ed;
            }


            //get next waypoint
            current_wp_id++;

            if(current_wp_id < current_count) {
                sendWaypointRequest(current_wp_id);
            } else {
                sendWaypointAck(0);

                // all waypoints retrieved, change state to idle
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;