Waypoint.cc 8.07 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
{
}

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

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

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

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

101

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

pixhawk's avatar
pixhawk committed
109
void Waypoint::setX(double x)
pixhawk's avatar
pixhawk committed
110
{
111
112
    if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
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
121
    if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
122
123
124
        this->y = y;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
125
126
}

pixhawk's avatar
pixhawk committed
127
void Waypoint::setZ(double z)
pixhawk's avatar
pixhawk committed
128
{
129
130
    if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
    {
131
132
133
        this->z = z;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
134
135
}

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

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

void Waypoint::setAltitude(double altitude)
{
156
157
    if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
    {
158
159
160
161
162
        this->z = altitude;
        emit changed(this);
    }
}

lm's avatar
lm committed
163
164
void Waypoint::setYaw(int yaw)
{
165
166
    if (this->yaw != yaw)
    {
lm's avatar
lm committed
167
168
169
170
171
        this->yaw = yaw;
        emit changed(this);
    }
}

pixhawk's avatar
pixhawk committed
172
void Waypoint::setYaw(double yaw)
pixhawk's avatar
pixhawk committed
173
{
174
175
    if (this->yaw != yaw)
    {
176
177
178
        this->yaw = yaw;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
179
180
}

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

lm's avatar
lm committed
190
void Waypoint::setAction(MAV_CMD action)
James Goppert's avatar
James Goppert committed
191
{
192
    if (this->action != action) {
193
        this->action = action;
194
195
196

        // Flick defaults according to WP type

Lorenz Meier's avatar
Lorenz Meier committed
197
        if (this->action == MAV_CMD_NAV_TAKEOFF) {
198
199
200
201
            // We default to 15 degrees minimum takeoff pitch
            this->param1 = 15.0;
        }

202
203
        emit changed(this);
    }
204
205
206
207
}

void Waypoint::setFrame(MAV_FRAME frame)
{
208
    if (this->frame != frame) {
209
210
211
        this->frame = frame;
        emit changed(this);
    }
James Goppert's avatar
James Goppert committed
212
213
}

pixhawk's avatar
pixhawk committed
214
215
void Waypoint::setAutocontinue(bool autoContinue)
{
216
    if (this->autocontinue != autoContinue) {
217
218
219
        this->autocontinue = autoContinue;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
220
221
222
223
}

void Waypoint::setCurrent(bool current)
{
224
225
    if (this->current != current)
    {
226
        this->current = current;
227
228
        // The current waypoint index is handled by the list
        // and not part of the individual waypoint update state
229
    }
pixhawk's avatar
pixhawk committed
230
}
pixhawk's avatar
pixhawk committed
231

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

void Waypoint::setParam1(double param1)
{
243
244
    //// // qDebug() << "SENDER:" << QObject::sender();
    //// // qDebug() << "PARAM1 SET REQ:" << param1;
245
246
    if (this->param1 != param1)
    {
247
248
249
250
251
252
253
        this->param1 = param1;
        emit changed(this);
    }
}

void Waypoint::setParam2(double param2)
{
254
255
    if (this->param2 != param2)
    {
256
257
258
259
260
261
262
        this->param2 = param2;
        emit changed(this);
    }
}

void Waypoint::setParam3(double param3)
{
263
    if (this->orbit != param3) {
264
        this->orbit = param3;
265
266
267
268
269
270
        emit changed(this);
    }
}

void Waypoint::setParam4(double param4)
{
271
    if (this->yaw != param4) {
lm's avatar
lm committed
272
        this->yaw = param4;
273
274
275
276
277
278
        emit changed(this);
    }
}

void Waypoint::setParam5(double param5)
{
279
    if (this->x != param5) {
lm's avatar
lm committed
280
        this->x = param5;
281
282
283
284
285
286
        emit changed(this);
    }
}

void Waypoint::setParam6(double param6)
{
287
    if (this->y != param6) {
288
        this->y = param6;
289
290
291
292
        emit changed(this);
    }
}

lm's avatar
lm committed
293
294
void Waypoint::setParam7(double param7)
{
295
    if (this->z != param7) {
lm's avatar
lm committed
296
297
298
299
300
        this->z = param7;
        emit changed(this);
    }
}

301
void Waypoint::setLoiterOrbit(double orbit)
pixhawk's avatar
pixhawk committed
302
{
303
    if (this->orbit != orbit) {
304
305
306
        this->orbit = orbit;
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
307
308
309
310
}

void Waypoint::setHoldTime(int holdTime)
{
311
    if (this->param1 != holdTime) {
312
        this->param1 = holdTime;
313
314
315
316
        emit changed(this);
    }
}

lm's avatar
lm committed
317
318
void Waypoint::setHoldTime(double holdTime)
{
319
    if (this->param1 != holdTime) {
lm's avatar
lm committed
320
321
322
323
324
        this->param1 = holdTime;
        emit changed(this);
    }
}

325
326
void Waypoint::setTurns(int turns)
{
327
    if (this->param1 != turns) {
328
        this->param1 = turns;
329
330
        emit changed(this);
    }
pixhawk's avatar
pixhawk committed
331
}