Newer
Older
/*======================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
(c) 2009-2011 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
This file is part of the PIXHAWK project
PIXHAWK 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.
PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
========================================================================*/
/**
* @file
* @brief a program to manage waypoints and exchange them with the ground station
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Benjamin Knecht <bknecht@student.ethz.ch>
* @author Christian Schluchter <schluchc@ee.ethz.ch>
*/
#include <cmath>
#include "MAVLinkSimulationWaypointPlanner.h"
#include "QGC.h"
#include <QDebug>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
class PxMatrix3x3;
/**
* @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat.
*
*/
class PxVector3
{
public:
/** @brief standard constructor */
PxVector3(void) {}
/** @brief copy constructor */
PxVector3(const PxVector3 &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } }
/** @brief x,y,z constructor */
PxVector3(const float _x, const float _y, const float _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; }
/** @brief broadcast constructor */
PxVector3(const float _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } }
private:
/** @brief private constructor (not used here, for SSE compatibility) */
PxVector3(const float (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } }
public:
/** @brief assignment operator */
void operator= (const PxVector3 &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } }
/** @brief const element access */
float operator[] (const int i) const { return m_vec[i]; }
/** @brief element access */
float &operator[] (const int i) { return m_vec[i]; }
// === arithmetic operators ===
/** @brief element-wise negation */
friend PxVector3 operator- (const PxVector3 &v) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; }
friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; }
friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; }
friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; }
friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; }
friend void operator+= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } }
friend void operator-= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } }
friend void operator*= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } }
friend void operator/= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } }
friend PxVector3 operator+ (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; }
friend PxVector3 operator- (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; }
friend PxVector3 operator* (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; }
friend PxVector3 operator/ (const PxVector3 &l, float f) { PxVector3 ret; float inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; }
friend void operator+= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } }
friend void operator-= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } }
friend void operator*= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } }
friend void operator/= (PxVector3 &l, float f) { float inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } }
// === vector operators ===
/** @brief dot product */
float dot(const PxVector3 &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; }
/** @brief length squared of the vector */
float lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; }
/** @brief length of the vector */
float length(void) const { return sqrt(lengthSquared()); }
/** @brief cross product */
PxVector3 cross(const PxVector3 &v) const { return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); }
/** @brief normalizes the vector */
PxVector3 &normalize(void) { const float l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; }
friend class PxMatrix3x3;
protected:
float m_vec[3];
};
/**
* @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat.
*
*/
class PxVector3Double
{
public:
/** @brief standard constructor */
PxVector3Double(void) {}
/** @brief copy constructor */
PxVector3Double(const PxVector3Double &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } }
/** @brief x,y,z constructor */
PxVector3Double(const double _x, const double _y, const double _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; }
/** @brief broadcast constructor */
PxVector3Double(const double _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } }
private:
/** @brief private constructor (not used here, for SSE compatibility) */
PxVector3Double(const double (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } }
public:
/** @brief assignment operator */
void operator= (const PxVector3Double &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } }
/** @brief const element access */
double operator[] (const int i) const { return m_vec[i]; }
/** @brief element access */
double &operator[] (const int i) { return m_vec[i]; }
// === arithmetic operators ===
/** @brief element-wise negation */
friend PxVector3Double operator- (const PxVector3Double &v) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; }
friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; }
friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; }
friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; }
friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; }
friend void operator+= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } }
friend void operator-= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } }
friend void operator*= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } }
friend void operator/= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } }
friend PxVector3Double operator+ (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; }
friend PxVector3Double operator- (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; }
friend PxVector3Double operator* (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; }
friend PxVector3Double operator/ (const PxVector3Double &l, double f) { PxVector3Double ret; double inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; }
friend void operator+= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } }
friend void operator-= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } }
friend void operator*= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } }
friend void operator/= (PxVector3Double &l, double f) { double inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } }
// === vector operators ===
/** @brief dot product */
double dot(const PxVector3Double &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; }
/** @brief length squared of the vector */
double lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; }
/** @brief length of the vector */
double length(void) const { return sqrt(lengthSquared()); }
/** @brief cross product */
PxVector3Double cross(const PxVector3Double &v) const { return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); }
/** @brief normalizes the vector */
PxVector3Double &normalize(void) { const double l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; }
friend class PxMatrix3x3;
protected:
double m_vec[3];
};
MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int sysid) :
QObject(parent),
link(parent),
idle(false),
current_active_wp_id(-1),
timestamp_lastoutside_orbit(0),
timestamp_firstinside_orbit(0),
waypoints(&waypoints1),
waypoints_receive_buffer(&waypoints2),
current_state(PX_WPP_IDLE),
protocol_current_wp_id(0),
protocol_current_count(0),
protocol_current_partner_systemid(0),
protocol_current_partner_compid(0),
protocol_timestamp_lastaction(0),
protocol_timeout(1000),
timestamp_last_send_setpoint(0),
systemid(sysid),
compid(MAV_COMP_ID_WAYPOINTPLANNER),
setpointDelay(10),
yawTolerance(0.4f),
verbose(true),
debug(false),
silent(false)
{
connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED";
}
/*
* @brief Sends an waypoint ack message
*/
void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_waypoint_ack_t wpa;
wpa.target_system = target_systemid;
wpa.target_component = target_compid;
wpa.type = type;
mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
}
/*
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
{
if(seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
mavlink_message_t msg;
mavlink_waypoint_current_t wpc;
wpc.seq = cur->seq;
mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq);
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
}
}
/*
* @brief Directs the MAV to fly to a position
*
* Sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
{
if(seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
mavlink_message_t msg;
mavlink_local_position_setpoint_set_t PControlSetPoint;
// send new set point to local IMU
if (cur->frame == 1)
{
PControlSetPoint.target_system = systemid;
PControlSetPoint.target_component = MAV_COMP_ID_IMU;
PControlSetPoint.x = cur->x;
PControlSetPoint.y = cur->y;
PControlSetPoint.z = cur->z;
mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
link->sendMAVLinkMessage(&msg);
}
else
{
//if (verbose) qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
PControlSetPoint.target_system = systemid;
PControlSetPoint.target_component = MAV_COMP_ID_IMU;
PControlSetPoint.x = cur->x;
PControlSetPoint.y = cur->y;
PControlSetPoint.z = cur->z;
mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
link->sendMAVLinkMessage(&msg);
emit messageSent(msg);
}
uint64_t now = QGC::groundTimeUsecs()/1000;
timestamp_last_send_setpoint = now;
}
}
void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)
{
mavlink_message_t msg;
mavlink_waypoint_count_t wpc;
wpc.target_system = target_systemid;
wpc.target_component = target_compid;
wpc.count = count;
mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
}
void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
{
if (seq < waypoints->size())
{
mavlink_message_t msg;
mavlink_waypoint_t *wp = waypoints->at(seq);
wp->target_system = target_systemid;
wp->target_component = target_compid;
if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue);
mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
}
else
{
if (verbose) qDebug("ERROR: index out of bounds\n");
}
}
void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
{
mavlink_message_t msg;
mavlink_waypoint_request_t wpr;
wpr.target_system = target_systemid;
wpr.target_component = target_compid;
wpr.seq = seq;
mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
}
/*
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_waypoint_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
link->sendMAVLinkMessage(&msg);
if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq);
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
}
float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z)
{
if (seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
const PxVector3 A(cur->x, cur->y, cur->z);
const PxVector3 C(x, y, z);
// seq not the second last waypoint
if ((uint16_t)(seq+1) < waypoints->size())
{
mavlink_waypoint_t *next = waypoints->at(seq+1);
const PxVector3 B(next->x, next->y, next->z);
const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
if (r >= 0 && r <= 1)
{
const PxVector3 P(A + r*(B-A));
return (P-C).length();
}
else if (r < 0.f)
{
return (C-A).length();
}
else
{
return (C-B).length();
}
}
else
{
return (C-A).length();
}
}
return -1.f;
}
float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z)
{
if (seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
const PxVector3 A(cur->x, cur->y, cur->z);
const PxVector3 C(x, y, z);
return (C-A).length();
}
return -1.f;
}
float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y)
{
if (seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
const PxVector3 A(cur->x, cur->y, 0);
const PxVector3 C(x, y, 0);
return (C-A).length();
}
return -1.f;
}
void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
{
mavlink_handler(&msg);
}
void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg)
{
// Handle param messages
// paramClient->handleMAVLinkPacket(msg);
//check for timed-out operations
//qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
uint64_t now = QGC::groundTimeUsecs()/1000;
if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
{
if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
current_state = PX_WPP_IDLE;
protocol_current_count = 0;
protocol_current_partner_systemid = 0;
protocol_current_partner_compid = 0;
protocol_current_wp_id = -1;
if(waypoints->size() == 0)
{
current_active_wp_id = -1;
}
}
if(now-timestamp_last_send_setpoint > setpointDelay)
{
send_setpoint(current_active_wp_id);
}
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = yawTolerance;
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
yawReached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
yawReached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
yawReached = true;
}
// FIXME HACK: Ignore yaw:
yawReached = true;
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos);
//qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z;
posReached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
if (wp->param2 == 0)
{
dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
}
else
{
dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
}
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 0)
{
mavlink_global_position_int_t pos;
mavlink_msg_global_position_int_decode(msg, &pos);
float x = static_cast<double>(pos.lon)/1E7;
float y = static_cast<double>(pos.lat)/1E7;
float z = static_cast<double>(pos.alt)/1000;
qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
posReached = false;
yawReached = true;
// FIXME big hack for simulation!
//float oneDegreeOfLatMeters = 111131.745f;
// compare current position (given in message) with current waypoint
//float orbit = wp->param1;
// Convert to degrees
float dist;
dist = distanceToPoint(current_active_wp_id, x, y);
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
qDebug() << "WP PLANNER: REACHED POSITION";
}
}
}
break;
}
case MAVLINK_MSG_ID_ACTION: // special action from ground station
{
mavlink_action_t action;
mavlink_msg_action_decode(msg, &action);
if(action.target == systemid)
{
if (verbose) qDebug("Waypoint: received message with action %d\n", action.action);
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
switch (action.action)
{
// case MAV_ACTION_LAUNCH:
// if (verbose) std::cerr << "Launch received" << std::endl;
// current_active_wp_id = 0;
// if (waypoints->size()>0)
// {
// setActive(waypoints[current_active_wp_id]);
// }
// else
// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// break;
// case MAV_ACTION_CONTINUE:
// if (verbose) std::c
// err << "Continue received" << std::endl;
// idle = false;
// setActive(waypoints[current_active_wp_id]);
// break;
// case MAV_ACTION_HALT:
// if (verbose) std::cerr << "Halt received" << std::endl;
// idle = true;
// break;
// default:
// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// break;
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
mavlink_waypoint_ack_t wpa;
mavlink_msg_waypoint_ack_decode(msg, &wpa);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid))
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (protocol_current_wp_id == waypoints->size()-1)
{
if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
current_state = PX_WPP_IDLE;
protocol_current_wp_id = 0;
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
mavlink_waypoint_set_current_t wpc;
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE)
{
if (wpc.seq < waypoints->size())
{
if (verbose) qDebug("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (i == current_active_wp_id)
{
waypoints->at(i)->current = true;
}
else
{
waypoints->at(i)->current = false;
}
}
if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
}
else
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
}
}
}
else
{
qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid;
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
mavlink_waypoint_request_list_t wprl;
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
if(wprl.target_system == systemid && wprl.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST)
{
if (waypoints->size() > 0)
{
if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
current_state = PX_WPP_SENDLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
}
else
{
if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
protocol_current_count = waypoints->size();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
}
else
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
}
}
else
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n");
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
mavlink_waypoint_request_t wpr;
mavlink_msg_waypoint_request_decode(msg, &wpr);
if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid)
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
{
if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
current_state = PX_WPP_SENDLIST_SENDWPS;
protocol_current_wp_id = wpr.seq;
send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
}
else
{
if (verbose)
{
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
else if (current_state == PX_WPP_SENDLIST)
{
if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
}
else if (current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
}
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
mavlink_waypoint_count_t wpc;
mavlink_msg_waypoint_count_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0))
{
if (wpc.count > 0)
{
if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
protocol_current_count = wpc.count;
qDebug("clearing receive buffer and readying for receiving waypoints\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints_receive_buffer->back();
waypoints_receive_buffer->pop_back();
}
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
else
{
if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
}
}
else
{
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT:
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid))
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
{
if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1;
mavlink_waypoint_t* newwp = new mavlink_waypoint_t;
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
waypoints_receive_buffer->push_back(newwp);
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
{
if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
if (current_active_wp_id > waypoints_receive_buffer->size()-1)
{
current_active_wp_id = waypoints_receive_buffer->size() - 1;
}
// switch the waypoints list
std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints;
waypoints = waypoints_receive_buffer;
waypoints_receive_buffer = waypoints_temp;
//get the new current waypoint
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (waypoints->at(i)->current == 1)
{
current_active_wp_id = i;
//if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
break;
}
}
if (i == waypoints->size())
{
current_active_wp_id = -1;
yawReached = false;
posReached = false;
timestamp_firstinside_orbit = 0;
}
current_state = PX_WPP_IDLE;
}
else
{
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
}
else
{
if (current_state == PX_WPP_IDLE)
{
//we're done receiving waypoints, answer with ack.
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
qDebug("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
}
if (verbose)
{
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
else if (current_state == PX_WPP_GETLIST)
{
if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else if (current_state == PX_WPP_GETLIST_GETWPS)
{
if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
}
else
{
// We're target but already communicating with someone else
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
}
else if(wp.target_system == systemid && wp.target_component == compid)
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
mavlink_waypoint_clear_all_t wpca;
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE)
{
protocol_timestamp_lastaction = now;
if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
while(waypoints->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
}
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
{
if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
}
break;
}
default:
{
if (debug) qDebug("Waypoint: received message of unknown type\n");
break;
}
}
//check if the current waypoint was reached
if ((posReached && /*yawReached &&*/ !idle))
{
if (current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id);
if (timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq);