Waypoint.cc 8.07 KiB
Newer Older
/*===================================================================
pixhawk's avatar
pixhawk committed
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
    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.
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
    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.
pixhawk's avatar
pixhawk committed

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

pixhawk's avatar
pixhawk committed
======================================================================*/

/**
 * @file
 *   @brief Waypoint class
 *
 *   @author Benjamin Knecht <mavteam@student.ethz.ch>
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
pixhawk's avatar
pixhawk committed
 *
 */

lm's avatar
lm committed
#include <QDebug>

#include "Waypoint.h"
pixhawk's avatar
pixhawk committed

lm's avatar
lm committed
Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _param1, double _param2, double _param3, double _param4,
                   bool _autocontinue, bool _current, MAV_FRAME _frame, MAV_CMD _action, const QString& _description)
      x(_x),
      y(_y),
      z(_z),
      yaw(_param4),
      frame(_frame),
      action(_action),
      autocontinue(_autocontinue),
      current(_current),
      orbit(_param3),
      param1(_param1),
      param2(_param2),
      name(QString("WP%1").arg(id, 2, 10, QChar('0'))),
lm's avatar
lm committed
      description(_description),
      reachedTime(0)
pixhawk's avatar
pixhawk committed
{
}

Waypoint::~Waypoint()
pixhawk's avatar
pixhawk committed
}

lm's avatar
lm committed
bool Waypoint::isNavigationType()
{
    return (action < MAV_CMD_NAV_LAST);
}

void Waypoint::save(QTextStream &saveStream)
{
lm's avatar
lm committed
    QString position("%1\t%2\t%3");
    position = position.arg(x, 0, 'g', 18);
    position = position.arg(y, 0, 'g', 18);
    position = position.arg(z, 0, 'g', 18);
lm's avatar
lm committed
    QString parameters("%1\t%2\t%3\t%4");
lm's avatar
lm committed
    parameters = parameters.arg(param1, 0, 'g', 18).arg(param2, 0, 'g', 18).arg(orbit, 0, 'g', 18).arg(yaw, 0, 'g', 18);
    // FORMAT: <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LONGITUDE> <PARAM6/Y/LATITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE> <DESCRIPTION>
lm's avatar
lm committed
    // as documented here: http://qgroundcontrol.org/waypoint_protocol
    saveStream << this->getId() << "\t" << this->getCurrent() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t"  << parameters << "\t" << position  << "\t" << this->getAutoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n";
}

bool Waypoint::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
lm's avatar
lm committed
    if (wpParams.size() == 12) {
lm's avatar
lm committed
        this->current = (wpParams[1].toInt() == 1 ? true : false);
        this->frame = (MAV_FRAME) wpParams[2].toInt();
        this->action = (MAV_CMD) wpParams[3].toInt();
        this->param1 = wpParams[4].toDouble();
        this->param2 = wpParams[5].toDouble();
        this->orbit = wpParams[6].toDouble();
        this->yaw = wpParams[7].toDouble();
        this->x = wpParams[8].toDouble();
        this->y = wpParams[9].toDouble();
        this->z = wpParams[10].toDouble();
lm's avatar
lm committed
        this->autocontinue = (wpParams[11].toInt() == 1 ? true : false);
void Waypoint::setId(quint16 id)
pixhawk's avatar
pixhawk committed
{
    this->id = id;
    this->name = QString("WP%1").arg(id, 2, 10, QChar('0'));
    emit changed(this);
pixhawk's avatar
pixhawk committed
}

pixhawk's avatar
pixhawk committed
void Waypoint::setX(double x)
pixhawk's avatar
pixhawk committed
{
    if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
        this->x = x;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

pixhawk's avatar
pixhawk committed
void Waypoint::setY(double y)
pixhawk's avatar
pixhawk committed
{
    if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
        this->y = y;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

pixhawk's avatar
pixhawk committed
void Waypoint::setZ(double z)
pixhawk's avatar
pixhawk committed
{
    if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
        this->z = z;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

void Waypoint::setLatitude(double lat)
{
    if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
    {
        this->x = lat;
        emit changed(this);
    }
}

void Waypoint::setLongitude(double lon)
{
    if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
    {
        this->y = lon;
        emit changed(this);
    }
}

void Waypoint::setAltitude(double altitude)
{
    if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
    {
        this->z = altitude;
        emit changed(this);
    }
}

lm's avatar
lm committed
void Waypoint::setYaw(int yaw)
{
lm's avatar
lm committed
        this->yaw = yaw;
        emit changed(this);
    }
}

pixhawk's avatar
pixhawk committed
void Waypoint::setYaw(double yaw)
pixhawk's avatar
pixhawk committed
{
        this->yaw = yaw;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

void Waypoint::setAction(int action)
{
    if (this->action != (MAV_CMD)action)
    {
        this->action = (MAV_CMD)action;
void Waypoint::setAction(MAV_CMD action)
James Goppert's avatar
James Goppert committed
{
    if (this->action != action) {
        this->action = action;

        // Flick defaults according to WP type

        if (this->action == MAV_CMD_NAV_TAKEOFF) {
            // We default to 15 degrees minimum takeoff pitch
            this->param1 = 15.0;
        }

        emit changed(this);
    }
}

void Waypoint::setFrame(MAV_FRAME frame)
{
    if (this->frame != frame) {
        this->frame = frame;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
void Waypoint::setAutocontinue(bool autoContinue)
{
    if (this->autocontinue != autoContinue) {
        this->autocontinue = autoContinue;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

void Waypoint::setCurrent(bool current)
{
    if (this->current != current)
    {
        this->current = current;
        // The current waypoint index is handled by the list
        // and not part of the individual waypoint update state
pixhawk's avatar
pixhawk committed
}
void Waypoint::setAcceptanceRadius(double radius)
{
    if (this->param2 != radius)
    {
        emit changed(this);
    }
}

void Waypoint::setParam1(double param1)
{
    //// // qDebug() << "SENDER:" << QObject::sender();
    //// // qDebug() << "PARAM1 SET REQ:" << param1;
    if (this->param1 != param1)
    {
        this->param1 = param1;
        emit changed(this);
    }
}

void Waypoint::setParam2(double param2)
{
    if (this->param2 != param2)
    {
        this->param2 = param2;
        emit changed(this);
    }
}

void Waypoint::setParam3(double param3)
{
    if (this->orbit != param3) {
        emit changed(this);
    }
}

void Waypoint::setParam4(double param4)
{
    if (this->yaw != param4) {
lm's avatar
lm committed
        this->yaw = param4;
        emit changed(this);
    }
}

void Waypoint::setParam5(double param5)
{
    if (this->x != param5) {
lm's avatar
lm committed
        this->x = param5;
        emit changed(this);
    }
}

void Waypoint::setParam6(double param6)
{
    if (this->y != param6) {
lm's avatar
lm committed
void Waypoint::setParam7(double param7)
{
    if (this->z != param7) {
lm's avatar
lm committed
        this->z = param7;
        emit changed(this);
    }
}

void Waypoint::setLoiterOrbit(double orbit)
pixhawk's avatar
pixhawk committed
{
    if (this->orbit != orbit) {
        this->orbit = orbit;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}

void Waypoint::setHoldTime(int holdTime)
{
    if (this->param1 != holdTime) {
lm's avatar
lm committed
void Waypoint::setHoldTime(double holdTime)
{
    if (this->param1 != holdTime) {
lm's avatar
lm committed
        this->param1 = holdTime;
        emit changed(this);
    }
}

void Waypoint::setTurns(int turns)
{
    if (this->param1 != turns) {
        this->param1 = turns;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
}