diff --git a/lib/QMapControl/src/layermanager.cpp b/lib/QMapControl/src/layermanager.cpp index 3358605..7e86192 100644 --- a/lib/QMapControl/src/layermanager.cpp +++ b/lib/QMapControl/src/layermanager.cpp @@ -229,7 +229,7 @@ namespace qmapcontrol if (clearImage) { - composedOffscreenImage2.fill(Qt::white); + composedOffscreenImage2.fill(Qt::black); } QPainter painter(&composedOffscreenImage2); @@ -428,6 +428,20 @@ namespace qmapcontrol forceRedraw(); } + void LayerManager::drawGeoms() + { + QPainter painter(&composedOffscreenImage); + QListIterator it(mylayers); + while (it.hasNext()) + { + Layer* l = it.next(); + if (l->layertype() == Layer::GeometryLayer && l->isVisible()) + { + l->drawYourGeometries(&painter, mapmiddle_px, layer()->offscreenViewport()); + } + } + } + void LayerManager::drawGeoms(QPainter* painter) { QListIterator it(mylayers); @@ -440,6 +454,8 @@ namespace qmapcontrol } } } + + void LayerManager::drawImage(QPainter* painter) { painter->drawPixmap(-scroll.x()-screenmiddle.x(), diff --git a/lib/QMapControl/src/layermanager.h b/lib/QMapControl/src/layermanager.h index c11a1ca..e2d83e0 100644 --- a/lib/QMapControl/src/layermanager.h +++ b/lib/QMapControl/src/layermanager.h @@ -166,6 +166,8 @@ namespace qmapcontrol */ int currentZoom() const; + + void drawGeoms(); void drawGeoms(QPainter* painter); void drawImage(QPainter* painter); diff --git a/lib/QMapControl/src/mapcontrol.cpp b/lib/QMapControl/src/mapcontrol.cpp index c01a95e..64125bd 100644 --- a/lib/QMapControl/src/mapcontrol.cpp +++ b/lib/QMapControl/src/mapcontrol.cpp @@ -324,6 +324,12 @@ namespace qmapcontrol { update(rect); } + + void MapControl::drawGeometries() + { + layermanager->drawGeoms(); + } + void MapControl::updateRequestNew() { // qDebug() << "MapControl::updateRequestNew()"; diff --git a/lib/QMapControl/src/mapcontrol.h b/lib/QMapControl/src/mapcontrol.h index db790f4..af9f592 100644 --- a/lib/QMapControl/src/mapcontrol.h +++ b/lib/QMapControl/src/mapcontrol.h @@ -342,6 +342,8 @@ namespace qmapcontrol */ void updateRequest ( QRect rect ); + void drawGeometries(); + //! updates the hole map by creating a new offscreen image /*! * diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 81789c5..2b1b424 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -261,7 +261,9 @@ HEADERS += src/MG.h \ src/ui/designer/QGCParamSlider.h \ src/ui/designer/QGCActionButton.h \ src/ui/designer/QGCToolWidgetItem.h \ - src/ui/QGCMAVLinkLogPlayer.h + src/ui/QGCMAVLinkLogPlayer.h \ + src/comm/MAVLinkSimulationWaypointPlanner.h \ + src/comm/MAVLinkSimulationMAV.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008: { @@ -383,7 +385,9 @@ SOURCES += src/main.cc \ src/ui/designer/QGCParamSlider.cc \ src/ui/designer/QGCActionButton.cc \ src/ui/designer/QGCToolWidgetItem.cc \ - src/ui/QGCMAVLinkLogPlayer.cc + src/ui/QGCMAVLinkLogPlayer.cc \ + src/comm/MAVLinkSimulationWaypointPlanner.cc \ + src/comm/MAVLinkSimulationMAV.cc macx|win32-msvc2008: { SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 952bffe..7adc936 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -40,9 +40,9 @@ This file is part of the QGROUNDCONTROL project #include "LinkManager.h" #include "MAVLinkProtocol.h" #include "MAVLinkSimulationLink.h" -// MAVLINK includes -#include +#include "QGCMAVLink.h" #include "QGC.h" +#include "MAVLinkSimulationMAV.h" /** * Create a simulated link. This link is connected to an input and output file. @@ -90,15 +90,13 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile this->name = "MAVLink simulation link"; } + + // Initialize the pseudo-random number generator srand(QTime::currentTime().msec()); maxTimeNoise = 0; this->id = getNextLinkId(); LinkManager::instance()->add(this); - - // Open packet log - mavlinkLogFile = new QFile(); - //mavlinkLogFile->open(QIODevice::ReadOnly); } MAVLinkSimulationLink::~MAVLinkSimulationLink() @@ -118,6 +116,8 @@ void MAVLinkSimulationLink::run() status.motor_block = 1; status.packet_drop = 0; + + forever { @@ -146,6 +146,23 @@ void MAVLinkSimulationLink::run() } } +void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) +{ + // Allocate buffer with packet data + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); + + // Pack to link buffer + readyBufferMutex.lock(); + for (int i = 0; i < bufferlength; i++) + { + readyBuffer.enqueue(*(buf + i)); + } + readyBufferMutex.unlock(); + + //qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid; +} + void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg) { // Allocate buffer with packet data @@ -162,10 +179,7 @@ void MAVLinkSimulationLink::mainloop() // Test for encoding / decoding packets // Test data stream - const int streamlength = 4096; - int streampointer = 0; - //const int testoffset = 0; - uint8_t stream[streamlength] = {}; + streampointer = 0; // Fake system values @@ -673,7 +687,7 @@ void MAVLinkSimulationLink::mainloop() }*/ readyBufferMutex.lock(); - for (int i = 0; i < streampointer; i++) + for (unsigned int i = 0; i < streampointer; i++) { readyBuffer.enqueue(*(stream + i)); } @@ -717,6 +731,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) if (mavlink_parse_char(this->id, data[i], &msg, &comm)) { // MESSAGE RECEIVED! + qDebug() << "SIMULATION LINK RECEIVED MESSAGE!"; + emit messageReceived(msg); switch (msg.msgid) { @@ -937,6 +953,7 @@ bool MAVLinkSimulationLink::connect() emit connected(true); start(LowPriority); + MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1); // timer->start(rate); return true; } diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 383cb71..e4c541e 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -89,6 +89,7 @@ public slots: virtual void mainloop(); bool connectLink(bool connect); void connectLink(); + void sendMAVLinkMessage(const mavlink_message_t* msg); protected: @@ -106,7 +107,7 @@ protected: QString simulationHeader; /** File where the commands sent by the groundstation are stored **/ QFile* receiveFile; - QTextStream stream; + QTextStream textStream; QTextStream* fileStream; QTextStream* outStream; /** Buffer which can be read from connected protocols through readBytes(). **/ @@ -115,6 +116,10 @@ protected: quint64 rate; int maxTimeNoise; quint64 lastSent; + static const int streamlength = 4096; + unsigned int streampointer; + //const int testoffset = 0; + uint8_t stream[streamlength]; int readyBytes; QQueue readyBuffer; @@ -133,6 +138,7 @@ protected: signals: void valueChanged(int uasId, QString curve, double value, quint64 usec); + void messageReceived(const mavlink_message_t& message); }; diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc new file mode 100644 index 0000000..f2b881e --- /dev/null +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -0,0 +1,33 @@ +#include + +#include "MAVLinkSimulationMAV.h" + +MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) : + QObject(parent), + link(parent), + planner(parent, systemid), + systemid(systemid) +{ + connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); + mainloopTimer.start(1000); + connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); + mainloop(); +} + +void MAVLinkSimulationMAV::mainloop() +{ + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA); + link->sendMAVLinkMessage(&msg); +} + +void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) +{ + qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; + + switch(msg.msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + break; + } +} diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h new file mode 100644 index 0000000..eb9f80e --- /dev/null +++ b/src/comm/MAVLinkSimulationMAV.h @@ -0,0 +1,31 @@ +#ifndef MAVLINKSIMULATIONMAV_H +#define MAVLINKSIMULATIONMAV_H + +#include +#include + +#include "MAVLinkSimulationLink.h" +#include "MAVLinkSimulationWaypointPlanner.h" + +class MAVLinkSimulationMAV : public QObject +{ + Q_OBJECT +public: + explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid); + +signals: + +public slots: + void mainloop(); + void handleMessage(const mavlink_message_t& msg); + +protected: + MAVLinkSimulationLink* link; + MAVLinkSimulationWaypointPlanner planner; + int systemid; + QTimer mainloopTimer; + + +}; + +#endif // MAVLINKSIMULATIONMAV_H diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc new file mode 100644 index 0000000..9ef3e8b --- /dev/null +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -0,0 +1,958 @@ +/*====================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009-2011 PIXHAWK PROJECT + +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 . + +========================================================================*/ + +/** +* @file +* @brief a program to manage waypoints and exchange them with the ground station +* +* @author Petri Tanskanen +* @author Benjamin Knecht +* @author Christian Schluchter +*/ + +#include + +#include "MAVLinkSimulationWaypointPlanner.h" +#include "QGC.h" + +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))); +} + + + +/* +* @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) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +} + +/* +* @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) printf("Broadcasted new current waypoint %u\n", wpc.seq); + } +} + +/* +* @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; + PControlSetPoint.yaw = cur->yaw; + + mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); + link->sendMAVLinkMessage(&msg); + + + } + else + { + if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + uint64_t now = QGC::groundTimeUsecs(); + 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) printf("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) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); + + mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); + link->sendMAVLinkMessage(&msg); + if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + + } + else + { + if (verbose) printf("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) printf("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) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + +} + +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; +} + +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 + + printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid); + + uint64_t now = QGC::groundTimeUsecs(); + if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); + 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->yaw && att.yaw + yaw_tolerance >= wp->yaw) + yawReached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->yaw || wp->yaw < att.yaw + yaw_tolerance) + yawReached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound) + 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); + if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, 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_ACTION: // special action from ground station + { + mavlink_action_t action; + mavlink_msg_action_decode(msg, &action); + if(action.target == systemid) + { + if (verbose) printf("Waypoint: received message with action %d\n", action.action); + 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) printf("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) printf("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) printf("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) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + } + 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) printf("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) printf("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) printf("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) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); + } + } + else + { + if (verbose) printf("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) printf("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) printf("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) printf("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)) { printf("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) printf("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) printf("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()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("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) printf("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) printf("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) printf("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; + + printf("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) printf("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)) printf("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) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); + else printf("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) printf("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) printf("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) printf("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) printf("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* 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) printf("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; + 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); + printf("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)) { printf("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)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("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)) printf("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)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we 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) printf("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) printf("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) printf("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) printf("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) printf("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) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + send_waypoint_reached(cur_wp->seq); + timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + current_active_wp_id = 1; + } + else + { + current_active_wp_id++; + } + + // Fly to next waypoint + timestamp_firstinside_orbit = 0; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + waypoints->at(current_active_wp_id)->current = true; + posReached = false; + //yawReached = false; + if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); + } + } + } + } + else + { + timestamp_lastoutside_orbit = now; + } +} diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.h b/src/comm/MAVLinkSimulationWaypointPlanner.h new file mode 100644 index 0000000..05176d3 --- /dev/null +++ b/src/comm/MAVLinkSimulationWaypointPlanner.h @@ -0,0 +1,74 @@ +#ifndef MAVLINKSIMULATIONWAYPOINTPLANNER_H +#define MAVLINKSIMULATIONWAYPOINTPLANNER_H + +#include +#include + +#include "MAVLinkSimulationLink.h" +#include "QGCMAVLink.h" + +enum PX_WAYPOINTPLANNER_STATES +{ + PX_WPP_IDLE = 0, + PX_WPP_SENDLIST, + PX_WPP_SENDLIST_SENDWPS, + PX_WPP_GETLIST, + PX_WPP_GETLIST_GETWPS, + PX_WPP_GETLIST_GOTALL +}; + +class MAVLinkSimulationWaypointPlanner : public QObject +{ + Q_OBJECT +public: + explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid); + +signals: + +public slots: + void handleMessage(const mavlink_message_t& msg); + +protected: + MAVLinkSimulationLink* link; + bool idle; ///< indicates if the system is following the waypoints or is waiting + uint16_t current_active_wp_id; ///< id of current waypoint + bool yawReached; ///< boolean for yaw attitude reached + bool posReached; ///< boolean for position reached + uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + + std::vector waypoints1; ///< vector1 that holds the waypoints + std::vector waypoints2; ///< vector2 that holds the waypoints + + std::vector* waypoints; ///< pointer to the currently active waypoint vector + std::vector* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector + PX_WAYPOINTPLANNER_STATES current_state; + uint16_t protocol_current_wp_id; + uint16_t protocol_current_count; + uint8_t protocol_current_partner_systemid; + uint8_t protocol_current_partner_compid; + uint64_t protocol_timestamp_lastaction; + unsigned int protocol_timeout; + uint64_t timestamp_last_send_setpoint; + uint8_t systemid; + uint8_t compid; + unsigned int setpointDelay; + float yawTolerance; + bool verbose; + bool debug; + bool silent; + + void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type); + void send_waypoint_current(uint16_t seq); + void send_setpoint(uint16_t seq); + void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count); + void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); + void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); + void send_waypoint_reached(uint16_t seq); + float distanceToSegment(uint16_t seq, float x, float y, float z); + float distanceToPoint(uint16_t seq, float x, float y, float z); + void mavlink_handler(const mavlink_message_t* msg); + +}; + +#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index c59aad2..7f9f50d 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -22,7 +22,8 @@ #endif -SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, ParityType parity, DataBitsType dataBits, StopBitsType stopBits) +SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, ParityType parity, DataBitsType dataBits, StopBitsType stopBits) : + port(NULL) { // Setup settings this->porthandle = portname.trimmed(); @@ -39,7 +40,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P this->id = getNextLinkId(); // *nix (Linux, MacOS tested) serial port support - port = new QextSerialPort(porthandle, QextSerialPort::Polling); +// port = new QextSerialPort(porthandle, QextSerialPort::Polling); //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); this->baudrate = baudrate; @@ -48,12 +49,12 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P this->dataBits = dataBits; this->stopBits = stopBits; this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. - port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time - port->setBaudRate(baudrate); - port->setFlowControl(flow); - port->setParity(parity); - port->setDataBits(dataBits); - port->setStopBits(stopBits); +// port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time +// port->setBaudRate(baudrate); +// port->setFlowControl(flow); +// port->setParity(parity); +// port->setDataBits(dataBits); +// port->setStopBits(stopBits); // Set the port name if (porthandle == "") @@ -153,7 +154,7 @@ void SerialLink::run() void SerialLink::checkForBytes() { /* Check if bytes are available */ - if(port->isOpen()) + if(port && port->isOpen()) { dataMutex.lock(); qint64 available = port->bytesAvailable(); @@ -174,7 +175,7 @@ void SerialLink::checkForBytes() void SerialLink::writeBytes(const char* data, qint64 size) { - if(port->isOpen()) + if(port && port->isOpen()) { int b = port->write(data, size); qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; @@ -201,7 +202,7 @@ void SerialLink::writeBytes(const char* data, qint64 size) void SerialLink::readBytes() { dataMutex.lock(); - if(port->isOpen()) + if(port && port->isOpen()) { const qint64 maxLength = 2048; char data[maxLength]; @@ -238,7 +239,14 @@ void SerialLink::readBytes() **/ qint64 SerialLink::bytesAvailable() { - return port->bytesAvailable(); + if (port) + { + return port->bytesAvailable(); + } + else + { + return 0; + } } /** @@ -248,23 +256,33 @@ qint64 SerialLink::bytesAvailable() **/ bool SerialLink::disconnect() { - //#if !defined _WIN32 || !defined _WIN64 - /* Block the thread until it returns from run() */ - //#endif - dataMutex.lock(); - port->flush(); - port->close(); - dataMutex.unlock(); + if (port) + { + //#if !defined _WIN32 || !defined _WIN64 + /* Block the thread until it returns from run() */ + //#endif +// dataMutex.lock(); + port->flush(); + port->close(); + delete port; + port = NULL; +// dataMutex.unlock(); - if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect + if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect - bool closed = true; - //port->isOpen(); + bool closed = true; + //port->isOpen(); - emit disconnected(); - emit connected(false); + emit disconnected(); + emit connected(false); + return ! closed; + } + else + { + // No port, so we're disconnected + return true; + } - return ! closed; } /** @@ -274,21 +292,14 @@ bool SerialLink::disconnect() **/ bool SerialLink::connect() { - qDebug() << "CONNECTING LINK: " << __FILE__ << __LINE__ << "with settings" << porthandle << baudrate << dataBits << parity << stopBits; - if (!this->isRunning()) + if (!isConnected()) { - this->start(LowPriority); - } - else - { - if(isConnected()) + qDebug() << "CONNECTING LINK: " << __FILE__ << __LINE__ << "with settings" << porthandle << baudrate << dataBits << parity << stopBits; + if (!this->isRunning()) { - disconnect(); + this->start(LowPriority); } - hardwareConnect(); } - - return port->isOpen(); } /** @@ -301,6 +312,12 @@ bool SerialLink::connect() **/ bool SerialLink::hardwareConnect() { + if(port) + { + port->close(); + delete port; + } + port = new QextSerialPort(porthandle, QextSerialPort::Polling); QObject::connect(port, SIGNAL(aboutToClose()), this, SIGNAL(disconnected())); port->open(QIODevice::ReadWrite); @@ -308,10 +325,9 @@ bool SerialLink::hardwareConnect() port->setParity(this->parity); port->setStopBits(this->stopBits); port->setDataBits(this->dataBits); + port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time - statisticsMutex.lock(); connectionStartTime = MG::TIME::getGroundTimeNow(); - statisticsMutex.unlock(); bool connectionUp = isConnected(); if(connectionUp) @@ -580,10 +596,14 @@ bool SerialLink::setPortName(QString portName) if(portName.trimmed().length() > 0) { bool reconnect = false; - if(isConnected()) + if (port) { - disconnect(); - reconnect = true; + if (port->isOpen()) + { + reconnect = true; + } + port->close(); + delete port; } this->porthandle = portName.trimmed(); setName(tr("serial port ") + portName.trimmed()); @@ -596,7 +616,7 @@ bool SerialLink::setPortName(QString portName) this->porthandle = "\\\\.\\" + this->porthandle; } #endif - if(port) delete port; + port = new QextSerialPort(porthandle, QextSerialPort::Polling); port->setBaudRate(baudrate); diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 443ee5f..3a7324c 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -630,6 +630,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, "")); } + mc->drawGeometries(); + + //mc->updateRequest(QRect(QPoint(0, 0), QPoint(600, 600))); + //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index 3b11d8d..9dc23f9 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -28,7 +28,17 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) : if (dock) { dock->setWindowTitle(title); + dock->setObjectName(title+"DOCK"); } + + // Try with parent + dock = dynamic_cast(this->parentWidget()); + if (dock) + { + dock->setWindowTitle(title); + dock->setObjectName(title+"DOCK"); + } + this->setWindowTitle(title); QList systems = UASManager::instance()->getUASList();