Waypoint.cc 7.54 KB
Newer Older
1
/*===================================================================
pixhawk's avatar
pixhawk committed
2
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
3

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

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

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

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

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

pixhawk's avatar
pixhawk committed
21 22 23 24 25 26 27
======================================================================*/

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

32
#include <QStringList>
lm's avatar
lm committed
33 34 35
#include <QDebug>

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

lm's avatar
lm committed
37
Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _param1, double _param2, double _param3, double _param4,
38
                   bool _autocontinue, bool _current, MAV_FRAME _frame, MAV_CMD _action, const QString& _description)
39
    : id(_id),
40 41 42 43 44 45 46 47 48 49 50
      x(_x),
      y(_y),
      z(_z),
      yaw(_param4),
      frame(_frame),
      action(_action),
      autocontinue(_autocontinue),
      current(_current),
      orbit(_param3),
      param1(_param1),
      param2(_param2),
51
      name(QString("WP%1").arg(id, 2, 10, QChar('0'))),
lm's avatar
lm committed
52 53
      description(_description),
      reachedTime(0)
pixhawk's avatar
pixhawk committed
54 55 56 57 58
{
}

Waypoint::~Waypoint()
{
pixhawk's avatar
pixhawk committed
59

pixhawk's avatar
pixhawk committed
60 61
}

lm's avatar
lm committed
62 63 64 65 66
bool Waypoint::isNavigationType()
{
    return (action < MAV_CMD_NAV_LAST);
}

67 68
void Waypoint::save(QTextStream &saveStream)
{
lm's avatar
lm committed
69
    QString position("%1\t%2\t%3");
70 71 72
    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
73
    QString parameters("%1\t%2\t%3\t%4");
lm's avatar
lm committed
74
    parameters = parameters.arg(param1, 0, 'g', 18).arg(param2, 0, 'g', 18).arg(orbit, 0, 'g', 18).arg(yaw, 0, 'g', 18);
75
    // 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
76
    // as documented here: http://qgroundcontrol.org/waypoint_protocol
77
    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";
78 79 80 81 82
}

bool Waypoint::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
lm's avatar
lm committed
83
    if (wpParams.size() == 12) {
84
        this->id = wpParams[0].toInt();
lm's avatar
lm committed
85 86 87 88 89 90 91
        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();
92 93 94
        this->x = wpParams[8].toDouble();
        this->y = wpParams[9].toDouble();
        this->z = wpParams[10].toDouble();
lm's avatar
lm committed
95
        this->autocontinue = (wpParams[11].toInt() == 1 ? true : false);
96
        //this->description = wpParams[12];
97 98 99 100 101
        return true;
    }
    return false;
}

102

103
void Waypoint::setId(quint16 id)
pixhawk's avatar
pixhawk committed
104
{
105
    this->id = id;
106 107
    this->name = QString("WP%1").arg(id, 2, 10, QChar('0'));
    emit changed(this);
pixhawk's avatar
pixhawk committed
108 109
}

pixhawk's avatar
pixhawk committed
110
void Waypoint::setX(double x)
pixhawk's avatar
pixhawk committed
111
{
112
    if (this->x != x && (this->frame == MAV_FRAME_LOCAL)) {
113 114 115
        this->x = x;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
116 117
}

pixhawk's avatar
pixhawk committed
118
void Waypoint::setY(double y)
pixhawk's avatar
pixhawk committed
119
{
120
    if (this->y != y && (this->frame == MAV_FRAME_LOCAL)) {
121 122 123
        this->y = y;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
124 125
}

pixhawk's avatar
pixhawk committed
126
void Waypoint::setZ(double z)
pixhawk's avatar
pixhawk committed
127
{
128
    if (this->z != z && (this->frame == MAV_FRAME_LOCAL)) {
129 130 131
        this->z = z;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
132 133
}

134 135
void Waypoint::setLatitude(double lat)
{
136
    if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
137 138 139 140 141 142 143
        this->x = lat;
        emit changed(this);
    }
}

void Waypoint::setLongitude(double lon)
{
144
    if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
145 146 147 148 149 150 151
        this->y = lon;
        emit changed(this);
    }
}

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

lm's avatar
lm committed
158 159
void Waypoint::setYaw(int yaw)
{
160
    if (this->yaw != yaw) {
lm's avatar
lm committed
161 162 163 164 165
        this->yaw = yaw;
        emit changed(this);
    }
}

pixhawk's avatar
pixhawk committed
166
void Waypoint::setYaw(double yaw)
pixhawk's avatar
pixhawk committed
167
{
168
    if (this->yaw != yaw) {
169 170 171
        this->yaw = yaw;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
172 173
}

174 175
void Waypoint::setAction(int action)
{
176
    if (this->action != (MAV_CMD)action) {
177
        this->action = (MAV_CMD)action;
178 179 180 181
        emit changed(this);
    }
}

182
void Waypoint::setAction(MAV_CMD action)
James Goppert's avatar
James Goppert committed
183
{
184
    if (this->action != action) {
185 186 187
        this->action = action;
        emit changed(this);
    }
188 189 190 191
}

void Waypoint::setFrame(MAV_FRAME frame)
{
192
    if (this->frame != frame) {
193 194 195
        this->frame = frame;
        emit changed(this);
    }
James Goppert's avatar
James Goppert committed
196 197
}

pixhawk's avatar
pixhawk committed
198 199
void Waypoint::setAutocontinue(bool autoContinue)
{
200
    if (this->autocontinue != autocontinue) {
201 202 203
        this->autocontinue = autoContinue;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
204 205 206 207
}

void Waypoint::setCurrent(bool current)
{
208
    if (this->current != current) {
209 210 211
        this->current = current;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
212
}
213

214 215
void Waypoint::setAcceptanceRadius(double radius)
{
216
    if (this->param2 != radius) {
217
        this->param2 = radius;
218 219 220 221 222 223
        emit changed(this);
    }
}

void Waypoint::setParam1(double param1)
{
lm's avatar
lm committed
224 225
    qDebug() << "SENDER:" << QObject::sender();
    qDebug() << "PARAM1 SET REQ:" << param1;
226
    if (this->param1 != param1) {
227 228 229 230 231 232 233
        this->param1 = param1;
        emit changed(this);
    }
}

void Waypoint::setParam2(double param2)
{
234
    if (this->param2 != param2) {
235 236 237 238 239 240 241
        this->param2 = param2;
        emit changed(this);
    }
}

void Waypoint::setParam3(double param3)
{
242
    if (this->orbit != param3) {
243
        this->orbit = param3;
244 245 246 247 248 249
        emit changed(this);
    }
}

void Waypoint::setParam4(double param4)
{
250
    if (this->yaw != param4) {
lm's avatar
lm committed
251
        this->yaw = param4;
252 253 254 255 256 257
        emit changed(this);
    }
}

void Waypoint::setParam5(double param5)
{
258
    if (this->x != param5) {
lm's avatar
lm committed
259
        this->x = param5;
260 261 262 263 264 265
        emit changed(this);
    }
}

void Waypoint::setParam6(double param6)
{
266
    if (this->y != param6) {
267
        this->y = param6;
268 269 270 271
        emit changed(this);
    }
}

lm's avatar
lm committed
272 273
void Waypoint::setParam7(double param7)
{
274
    if (this->z != param7) {
lm's avatar
lm committed
275 276 277 278 279
        this->z = param7;
        emit changed(this);
    }
}

280
void Waypoint::setLoiterOrbit(double orbit)
pixhawk's avatar
pixhawk committed
281
{
282
    if (this->orbit != orbit) {
283 284 285
        this->orbit = orbit;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
286 287 288 289
}

void Waypoint::setHoldTime(int holdTime)
{
290
    if (this->param1 != holdTime) {
291
        this->param1 = holdTime;
292 293 294 295
        emit changed(this);
    }
}

lm's avatar
lm committed
296 297
void Waypoint::setHoldTime(double holdTime)
{
298
    if (this->param1 != holdTime) {
lm's avatar
lm committed
299 300 301 302 303
        this->param1 = holdTime;
        emit changed(this);
    }
}

304 305
void Waypoint::setTurns(int turns)
{
306
    if (this->param1 != turns) {
307
        this->param1 = turns;
308 309
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
310
}