diff --git a/images/earth.html b/images/earth.html index 602799f14bdec5a7afe2db24ced99afcc85e5e87..c5699c596d2fdf2eff7948b77dc4780c19abaad8 100644 --- a/images/earth.html +++ b/images/earth.html @@ -71,6 +71,11 @@ function init() google.earth.createInstance("map3d", initCallback, failureCallback); } +function setFollowEnabled(enable) +{ + followEnabled = enable; +} + function setCurrAircraft(id) @@ -214,8 +219,8 @@ function initCallback(object) ge.getWindow().setVisibility(true); ge.getOptions().setStatusBarVisibility(true); ge.getOptions().setScaleLegendVisibility(true); - ge.getOptions().setFlyToSpeed(5.0); - //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); + //ge.getOptions().setFlyToSpeed(5.0); + ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO); ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); @@ -233,23 +238,31 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw) if (lastLat == 0) { lastLat = currLat; - lastLon = lastLon; + lastLon = currLon; } currLat = lat; currLon = lon; currAlt = alt; + // Interpolate between t-1 and t and set new states + lastLat = lastLat*0.8+currLat*0.2; + lastLon = lastLon*0.8+currLon*0.2; + lastAlt = lastAlt*0.8+currAlt*0.2; //currFollowHeading = ((yaw/M_PI)+1.0)*360.0; // FIXME Currently invalid conversion from right-handed z-down to z-up frame - planeOrient.setRoll(((roll/M_PI)+1.0)*360.0); - planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0); - planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0); + planeOrient.setRoll(((roll/M_PI))*180.0+180.0); + planeOrient.setTilt(((pitch/M_PI))*180.0+180.0); + planeOrient.setHeading(((yaw/M_PI))*180.0-90.0); + + currFollowHeading = ((yaw/M_PI))*180.0; - planeLoc.setLatitude(lat); - planeLoc.setLongitude(lon); - planeLoc.setAltitude(alt); + planeLoc.setLatitude(lastLat); + planeLoc.setLongitude(lastLon); + planeLoc.setAltitude(lastAlt); planeModel.setLocation(planeLoc); + + if (followEnabled) updateFollowAircraft(); } } @@ -284,17 +297,13 @@ function setCurrentAircraft(id) function updateFollowAircraft() { currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE); - // Interpolate between t-1 and t and set new states - lastLat = lastLat*0.7+currLat*0.3; - lastLon = lastLon*0.7+currLon*0.3; - lastAlt = lastAlt*0.7+currAlt*0.3; currView.setLatitude(lastLat); currView.setLongitude(lastLon); currView.setAltitude(lastAlt); currView.setRange(currViewRange); currView.setTilt(currFollowTilt); - currView.setHeading(currFollowHeading-90.0); + currView.setHeading(currFollowHeading); ge.getView().setAbstractView(currView); } diff --git a/lib/QMapControl/src/geometry.h b/lib/QMapControl/src/geometry.h index 2aefd955def97d20c30a15644b5fc10ae4420cb1..127ab799151aa9f0d6cea25830be5b5d25d32d71 100644 --- a/lib/QMapControl/src/geometry.h +++ b/lib/QMapControl/src/geometry.h @@ -119,7 +119,7 @@ namespace qmapcontrol virtual bool hasClickedPoints() const; virtual void setPen(QPen* pen); virtual QList clickedPoints(); - virtual QList points()=0; + virtual QList& points()=0; private: Geometry* myparentGeometry; diff --git a/lib/QMapControl/src/linestring.cpp b/lib/QMapControl/src/linestring.cpp index 0ca36d04215c3813ad71c3c0ade72851e17cdacf..0f60d9a1d91b8b2ee72a4a65af221fb327bd30b4 100644 --- a/lib/QMapControl/src/linestring.cpp +++ b/lib/QMapControl/src/linestring.cpp @@ -56,7 +56,12 @@ namespace qmapcontrol vertices.append(point); } - QList LineString::points() + void LineString::clearPoints() + { + vertices.clear(); + } + + QList& LineString::points() { return vertices; } diff --git a/lib/QMapControl/src/linestring.h b/lib/QMapControl/src/linestring.h index f00a58503f1d58907c79f00076d1597ef69b539b..355e568d2e93ca613bb21860b1df12c0464328fc 100644 --- a/lib/QMapControl/src/linestring.h +++ b/lib/QMapControl/src/linestring.h @@ -56,7 +56,7 @@ namespace qmapcontrol /*! * @return a list with the points of the LineString */ - QList points(); + QList& points(); //! adds a point at the end of the LineString /*! @@ -64,6 +64,9 @@ namespace qmapcontrol */ void addPoint ( Point* point ); + //! Remove all points from the LineString + void clearPoints(); + //! sets the given list as points of the LineString /*! * @param points the points which should be set for the LineString diff --git a/lib/QMapControl/src/point.cpp b/lib/QMapControl/src/point.cpp index 45793a628996d3f310c0424cb0b2760ed4f4894e..e15ca87cd4bd471bb1602a870577eb32d1e46541 100644 --- a/lib/QMapControl/src/point.cpp +++ b/lib/QMapControl/src/point.cpp @@ -293,12 +293,11 @@ namespace qmapcontrol emit(positionChanged(this)); } - QList Point::points() + QList& Point::points() { - //TODO: assigning temp?! - QList points; - points.append(this); - return points; + // FIXME TODO: THIS IS ABSOLUTELY WRONG IN THE ORIGINAL LIBRARY + // NEEDS AN INHERITANCE REWRITE!!!! + return m_points; } QWidget* Point::widget() diff --git a/lib/QMapControl/src/point.h b/lib/QMapControl/src/point.h index 426748a44a20a748c7533ca1574b63c62539d494..5ddadf7e157f7cc919eefa1fba973ce7c55a6e3b 100644 --- a/lib/QMapControl/src/point.h +++ b/lib/QMapControl/src/point.h @@ -139,7 +139,7 @@ namespace qmapcontrol */ QPointF coordinate() const; - virtual QList points(); + virtual QList& points(); /*! \brief returns the widget of the point @return the widget of the point @@ -191,6 +191,7 @@ namespace qmapcontrol QSize displaysize; QSize minsize; QSize maxsize; + QList m_points; void drawWidget(const MapAdapter* mapadapter, const QPoint offset); diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 13ff2dc06f452008305730b463382a5dcd452e27..44001d4edb043cc56a228a85399476df145cab39 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -140,7 +140,6 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCPxImuFirmwareUpdate.ui \ src/ui/QGCDataPlot2D.ui \ src/ui/QGCRemoteControlView.ui \ - src/ui/WaypointGlobalView.ui \ src/ui/QMap3D.ui \ src/ui/QGCWebView.ui \ src/ui/map3D/QGCGoogleEarthView.ui \ diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 782253e06ea1625600b5225df3569fa758a7a120..b629f3355a0205f99fc065d25cbd68f64d27b431 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -1,5 +1,4 @@ -/*===================================================================== - +/*=================================================================== QGroundControl Open Source Ground Control Station (c) 2009, 2010 QGROUNDCONTROL PROJECT @@ -33,18 +32,19 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include -Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action) -: id(_id), - x(_x), - y(_y), - z(_z), - yaw(_yaw), - frame(_frame), - action(_action), - autocontinue(_autocontinue), - current(_current), - orbit(_orbit), - holdTime(_holdTime) +Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action) + : id(_id), + x(_x), + y(_y), + z(_z), + yaw(_yaw), + frame(_frame), + action(_action), + autocontinue(_autocontinue), + current(_current), + orbit(_orbit), + holdTime(_holdTime), + name(QString("WP%1").arg(id, 2, 10, QChar('0'))) { } @@ -90,79 +90,141 @@ bool Waypoint::load(QTextStream &loadStream) void Waypoint::setId(quint16 id) { this->id = id; + this->name = QString("WP%1").arg(id, 2, 10, QChar('0')); + emit changed(this); } -void Waypoint::setX(float x) +void Waypoint::setX(double x) { - this->x = x; + if (this->x != x) + { + this->x = x; + emit changed(this); + } } -void Waypoint::setY(float y) +void Waypoint::setY(double y) { - this->y = y; + if (this->y != y) + { + this->y = y; + emit changed(this); + } } -void Waypoint::setZ(float z) +void Waypoint::setZ(double z) { - this->z = z; + if (this->z != z) + { + this->z = z; + emit changed(this); + } } -void Waypoint::setYaw(float yaw) +void Waypoint::setYaw(double yaw) { - this->yaw = yaw; + if (this->yaw != yaw) + { + this->yaw = yaw; + emit changed(this); + } } void Waypoint::setAction(MAV_ACTION action) { - this->action = action; + if (this->action != action) + { + this->action = action; + emit changed(this); + } } void Waypoint::setFrame(MAV_FRAME frame) { - this->frame = frame; + if (this->frame != frame) + { + this->frame = frame; + emit changed(this); + } } void Waypoint::setAutocontinue(bool autoContinue) { - this->autocontinue = autoContinue; + if (this->autocontinue != autocontinue) + { + this->autocontinue = autoContinue; + emit changed(this); + } } void Waypoint::setCurrent(bool current) { - this->current = current; + if (this->current != current) + { + this->current = current; + emit changed(this); + } } -void Waypoint::setOrbit(float orbit) +void Waypoint::setOrbit(double orbit) { - this->orbit = orbit; + if (this->orbit != orbit) + { + this->orbit = orbit; + emit changed(this); + } } void Waypoint::setHoldTime(int holdTime) { - this->holdTime = holdTime; -} - -void Waypoint::setX(double x) -{ - this->x = x; -} - -void Waypoint::setY(double y) -{ - this->y = y; -} - -void Waypoint::setZ(double z) -{ - this->z = z; -} - -void Waypoint::setYaw(double yaw) -{ - this->yaw = yaw; + if (this->holdTime != holdTime) + { + this->holdTime = holdTime; + emit changed(this); + } } -void Waypoint::setOrbit(double orbit) -{ - this->orbit = orbit; -} +//void Waypoint::setX(double x) +//{ +// if (this->x != static_cast(x)) +// { +// this->x = x; +// emit changed(this); +// } +//} + +//void Waypoint::setY(double y) +//{ +// if (this->y != static_cast(y)) +// { +// this->y = y; +// emit changed(this); +// } +//} + +//void Waypoint::setZ(double z) +//{ +// if (this->z != static_cast(z)) +// { +// this->z = z; +// emit changed(this); +// } +//} + +//void Waypoint::setYaw(double yaw) +//{ +// if (this->yaw != static_cast(yaw)) +// { +// this->yaw = yaw; +// emit changed(this); +// } +//} + +//void Waypoint::setOrbit(double orbit) +//{ +// if (this->orbit != static_cast(orbit)) +// { +// this->orbit = orbit; +// emit changed(this); +// } +//} diff --git a/src/Waypoint.h b/src/Waypoint.h index fa2246d5d5300ffa7c52ec3353e2e655cb45d3f1..afe529668108a4b7f260a49ced39659384fe20cf 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -43,22 +43,23 @@ class Waypoint : public QObject Q_OBJECT public: - Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, - bool current = false, float orbit = 0.15f, int holdTime = 0, + Waypoint(quint16 id = 0, double x = 0.0f, double y = 0.0f, double z = 0.0f, double yaw = 0.0f, bool autocontinue = false, + bool current = false, double orbit = 0.15f, int holdTime = 0, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE); ~Waypoint(); quint16 getId() const { return id; } - float getX() const { return x; } - float getY() const { return y; } - float getZ() const { return z; } - float getYaw() const { return yaw; } + double getX() const { return x; } + double getY() const { return y; } + double getZ() const { return z; } + double getYaw() const { return yaw; } bool getAutoContinue() const { return autocontinue; } bool getCurrent() const { return current; } - float getOrbit() const { return orbit; } - float getHoldTime() const { return holdTime; } + double getOrbit() const { return orbit; } + double getHoldTime() const { return holdTime; } MAV_FRAME getFrame() const { return frame; } MAV_ACTION getAction() const { return action; } + const QString& getName() const { return name; } void save(QTextStream &saveStream); bool load(QTextStream &loadStream); @@ -66,37 +67,42 @@ public: protected: quint16 id; - float x; - float y; - float z; - float yaw; + double x; + double y; + double z; + double yaw; MAV_FRAME frame; MAV_ACTION action; bool autocontinue; bool current; - float orbit; + double orbit; int holdTime; + QString name; public slots: void setId(quint16 id); - void setX(float x); - void setY(float y); - void setZ(float z); - void setYaw(float yaw); + void setX(double x); + void setY(double y); + void setZ(double z); + void setYaw(double yaw); void setAction(MAV_ACTION action); void setFrame(MAV_FRAME frame); void setAutocontinue(bool autoContinue); void setCurrent(bool current); - void setOrbit(float orbit); + void setOrbit(double orbit); void setHoldTime(int holdTime); - //for QDoubleSpin - void setX(double x); - void setY(double y); - void setZ(double z); - void setYaw(double yaw); - void setOrbit(double orbit); +// //for QDoubleSpin +// void setX(double x); +// void setY(double y); +// void setZ(double z); +// void setYaw(double yaw); +// void setOrbit(double orbit); + +signals: + /** @brief Announces a change to the waypoint data */ + void changed(Waypoint* wp); }; #endif // WAYPOINT_H diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 350cba49145db9df798c7ba02d14790ae095afd2..c8039c505aad981431205a87c18af5375235c3a3 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -154,12 +154,12 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); // Pack to link buffer -// readyBufferMutex.lock(); + readyBufferMutex.lock(); for (unsigned int i = 0; i < bufferlength; i++) { readyBuffer.enqueue(*(buf + i)); } -// readyBufferMutex.unlock(); + readyBufferMutex.unlock(); } void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg) @@ -451,6 +451,11 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; + // ATTITUDE VEHICLE 2 + mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0); + sendMAVLinkMessage(&ret); + + // // GLOBAL POSITION VEHICLE 3 // mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); // bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); @@ -627,7 +632,7 @@ void MAVLinkSimulationLink::mainloop() // HEARTBEAT VEHICLE 2 // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA); + messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -886,8 +891,7 @@ void MAVLinkSimulationLink::readBytes() { readyBufferMutex.lock(); const qint64 maxLength = 2048; char data[maxLength]; - qint64 len = maxLength; - if (maxLength > readyBuffer.size()) len = readyBuffer.size(); + qint64 len = qMin((qint64)readyBuffer.size(), maxLength); for (unsigned int i = 0; i < len; i++) { diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 6f9b7b9e639ea307856f7c4891fd9a72f1a65cc4..829f571c3f1cb06bae9c3753b1a12e36113336f6 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -4,32 +4,36 @@ #include "MAVLinkSimulationMAV.h" MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) : - QObject(parent), - link(parent), - planner(parent, systemid), - systemid(systemid), - timer25Hz(0), - timer10Hz(0), - timer1Hz(0), - latitude(0.0), - longitude(0.0), - altitude(0.0), - x(0.0), - y(0.0), - z(0.0), - roll(0.0), - pitch(0.0), - yaw(0.0), - globalNavigation(true), - firstWP(false), - previousSPX(0.0), - previousSPY(0.0), - previousSPZ(0.0), - previousSPYaw(0.0), - nextSPX(0.0), - nextSPY(0.0), - nextSPZ(0.0), - nextSPYaw(0.0) + QObject(parent), + link(parent), + planner(parent, systemid), + systemid(systemid), + timer25Hz(0), + timer10Hz(0), + timer1Hz(0), + latitude(47.376389), + longitude(8.548056), + altitude(0.0), + x(8.548056), + y(47.376389), + z(550), + roll(0.0), + pitch(0.0), + yaw(0.0), + globalNavigation(true), + firstWP(false), + previousSPX(8.548056), + previousSPY(47.376389), + previousSPZ(550), + previousSPYaw(0.0), + nextSPX(8.548056), + nextSPY(47.376389), + nextSPZ(550), + nextSPYaw(0.0), + sys_mode(MAV_MODE_READY), + sys_state(MAV_STATE_STANDBY), + nav_mode(MAV_NAV_GROUNDED), + flying(false) { // Please note: The waypoint planner is running connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); @@ -42,28 +46,15 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy void MAVLinkSimulationMAV::mainloop() { // Calculate new simulator values -// double maxSpeed = 0.0001; // rad/s in earth coordinate frame + // double maxSpeed = 0.0001; // rad/s in earth coordinate frame -// double xNew = // (nextSPX - previousSPX) + // double xNew = // (nextSPX - previousSPX) - if (!firstWP) + if (flying) { - double xm = (nextSPX - x) * 0.01; - double ym = (nextSPY - y) * 0.01; - double zm = (nextSPZ - z) * 0.1; - - x += xm; - y += ym; - z += zm; - - //if (xm < 0.001) xm - } - else - { - x = nextSPX; - y = nextSPY; - z = nextSPZ; - firstWP = false; + sys_state = MAV_STATE_ACTIVE; + sys_mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; } // 1 Hz execution @@ -72,12 +63,59 @@ void MAVLinkSimulationMAV::mainloop() mavlink_message_t msg; mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); link->sendMAVLinkMessage(&msg); + planner.handleMessage(msg); timer1Hz = 50; } // 10 Hz execution if (timer10Hz <= 0) { + if (!firstWP) + { + double radPer100ms = 0.00006; + double altPer100ms = 1.0; + double xm = (nextSPX - x); + double ym = (nextSPY - y); + double zm = (nextSPZ - z); + + float zsign = (zm < 0) ? -1.0f : 1.0f; + + //float trueyaw = atan2f(xm, ym); + + float newYaw = atan2f(xm, ym); + + if (fabs(yaw - newYaw) < 90) + { + yaw = yaw*0.7 + 0.3*newYaw; + } + else + { + yaw = newYaw; + } + + //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw; + + //if (sqrt(xm*xm+ym*ym) > 0.0000001) + if (flying) + { + x += sin(yaw)*radPer100ms; + y += cos(yaw)*radPer100ms; + z += altPer100ms*zsign; + } + + //if (xm < 0.001) xm + } + else + { + x = nextSPX; + y = nextSPY; + z = nextSPZ; + firstWP = false; + qDebug() << "INIT STEP"; + } + + + // GLOBAL POSITION mavlink_message_t msg; mavlink_global_position_int_t pos; pos.alt = z*1000.0; @@ -88,12 +126,90 @@ void MAVLinkSimulationMAV::mainloop() pos.vz = 0; mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); link->sendMAVLinkMessage(&msg); + planner.handleMessage(msg); + + // ATTITUDE + mavlink_attitude_t attitude; + attitude.roll = 0.0f; + attitude.pitch = 0.0f; + attitude.yaw = yaw; + + mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude); + link->sendMAVLinkMessage(&msg); + + // SYSTEM STATUS + mavlink_sys_status_t status; + status.load = 300; + status.motor_block = 1; + status.mode = sys_mode; + status.nav_mode = nav_mode; + status.packet_drop = 0; + status.vbat = 10500; + status.status = sys_state; + + mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status); + link->sendMAVLinkMessage(&msg); timer10Hz = 5; } // 25 Hz execution if (timer25Hz <= 0) { + + // Send a named value with name "FLOAT" and 0.5f as value + + // The message container to be used for sending + mavlink_message_t ret; + // The C-struct holding the data to be sent + mavlink_named_value_float_t val; + + // Fill in the data + // Name of the value, maximum 10 characters + // see full message specs at: + // http://pixhawk.ethz.ch/wiki/mavlink/ + strcpy(val.name, "FLOAT"); + // Value, in this case 0.5 + val.value = 0.5f; + + // Encode the data (adding header and checksums, etc.) + mavlink_msg_named_value_float_encode(systemid, MAV_COMP_ID_IMU, &ret, &val); + // And send it + link->sendMAVLinkMessage(&ret); + + // MICROCONTROLLER SEND CODE: + + // uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + // int16_t len = mavlink_msg_to_send_buffer(buf, &ret); + // uart0_transmit(buf, len); + + + // SEND INTEGER VALUE + + // We are reusing the "mavlink_message_t ret" + // message buffer + + // The C-struct holding the data to be sent + mavlink_named_value_int_t valint; + + // Fill in the data + // Name of the value, maximum 10 characters + // see full message specs at: + // http://pixhawk.ethz.ch/wiki/mavlink/ + strcpy(valint.name, "INTEGER"); + // Value, in this case 18000 + valint.value = 18000; + + // Encode the data (adding header and checksums, etc.) + mavlink_msg_named_value_int_encode(systemid, MAV_COMP_ID_IMU, &ret, &valint); + // And send it + link->sendMAVLinkMessage(&ret); + + // MICROCONTROLLER SEND CODE: + + // uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + // int16_t len = mavlink_msg_to_send_buffer(buf, &ret); + // uart0_transmit(buf, len); + timer25Hz = 2; } @@ -110,6 +226,37 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) { case MAVLINK_MSG_ID_ATTITUDE: break; + case MAVLINK_MSG_ID_SET_MODE: + { + mavlink_set_mode_t mode; + mavlink_msg_set_mode_decode(&msg, &mode); + if (systemid == mode.target) sys_mode = mode.mode; + } + break; + case MAVLINK_MSG_ID_ACTION: + { + mavlink_action_t action; + mavlink_msg_action_decode(&msg, &action); + if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) + { + switch (action.action) + { + case MAV_ACTION_LAUNCH: + flying = true; + break; + default: + { + mavlink_statustext_t text; + mavlink_message_t r_msg; + sprintf((char*)text.text, "MAV%d ignored unknown action %d", systemid, action.action); + mavlink_msg_statustext_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &text); + link->sendMAVLinkMessage(&r_msg); + } + break; + } + } + } + break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; @@ -127,9 +274,9 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) //nextSPYaw = sp.yaw; // Airplane - yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY); + //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY); - if (!firstWP) firstWP = true; + //if (!firstWP) firstWP = true; } //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ; } diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h index 5f92559c93c0e5f562a4ec9645e8897f1bf25b57..2a42598ff571dc09a30ba9bd6f184c041e4bc057 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -49,6 +49,10 @@ protected: double nextSPY; double nextSPZ; double nextSPYaw; + uint8_t sys_mode; + uint8_t sys_state; + uint8_t nav_mode; + bool flying; }; diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 43d1bb3cb93950ad9ad45930840d9ba798b70b05..645096b60abf00934f473ea4e619d5fcf402603b 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -437,6 +437,20 @@ float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, f 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); @@ -449,7 +463,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //check for timed-out operations - qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid; + //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) @@ -502,6 +516,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound) yawReached = true; } + + // FIXME HACK: Ignore yaw: + + yawReached = true; } } break; @@ -543,6 +561,49 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* 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(pos.lon)/1E7; + float y = static_cast(pos.lat)/1E7; + float z = static_cast(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; + float orbit = 0.00008; + + // 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; @@ -947,7 +1008,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { //the last waypoint was reached, if auto continue is //activated restart the waypoint list from the beginning - current_active_wp_id = 1; + current_active_wp_id = 0; } else { diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.h b/src/comm/MAVLinkSimulationWaypointPlanner.h index 12e2b066e3b8820a47f140f2855ff4896ee02e0a..363a255a706219edf773d37d4d6c0762fea4689d 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.h +++ b/src/comm/MAVLinkSimulationWaypointPlanner.h @@ -68,6 +68,7 @@ protected: 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); + float distanceToPoint(uint16_t seq, float x, float y); void mavlink_handler(const mavlink_message_t* msg); }; diff --git a/src/configuration.h b/src/configuration.h index e06ad87f0adc6ca998d39875662470d3fa29bdc3..51dec3d2b83e8f2472a2b40dd4a05e792fa46465 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -17,7 +17,7 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha)" +#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC6)" namespace QGC diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 873a6d98e09165a926ef8a3d6d0bacd96201108e..e5b78009b8771893ae2daa301c0b7d412c922148 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -146,18 +146,31 @@ void UAS::updateState() void UAS::setSelected() { - UASManager::instance()->setActiveUAS(this); + if (UASManager::instance()->getActiveUAS() != this) + { + UASManager::instance()->setActiveUAS(this); + emit systemSelected(true); + } +} + +bool UAS::getSelected() const +{ + return (UASManager::instance()->getActiveUAS() == this); } void UAS::receiveMessageNamedValue(const mavlink_message_t& message) { if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) { - + mavlink_named_value_float_t val; + mavlink_msg_named_value_float_decode(&message, &val); + emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0)); } else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) { - + mavlink_named_value_int_t val; + mavlink_msg_named_value_int_decode(&message, &val); + emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0)); } } @@ -181,6 +194,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) QString uasState; QString stateDescription; QString patternPath; + + // Receive named value message + receiveMessageNamedValue(message); + switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: @@ -326,15 +343,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_raw_imu_decode(&message, &raw); quint64 time = getUnixTime(raw.usec); - emit valueChanged(uasId, "accel x", "raw", raw.xacc, time); - emit valueChanged(uasId, "accel y", "raw", raw.yacc, time); - emit valueChanged(uasId, "accel z", "raw", raw.zacc, time); + emit valueChanged(uasId, "accel x", "raw", static_cast(raw.xacc), time); + emit valueChanged(uasId, "accel y", "raw", static_cast(raw.yacc), time); + emit valueChanged(uasId, "accel z", "raw", static_cast(raw.zacc), time); emit valueChanged(uasId, "gyro roll", "raw", static_cast(raw.xgyro), time); emit valueChanged(uasId, "gyro pitch", "raw", static_cast(raw.ygyro), time); emit valueChanged(uasId, "gyro yaw", "raw", static_cast(raw.zgyro), time); - emit valueChanged(uasId, "mag x", "raw", raw.xmag, time); - emit valueChanged(uasId, "mag y", "raw", raw.ymag, time); - emit valueChanged(uasId, "mag z", "raw", raw.zmag, time); + emit valueChanged(uasId, "mag x", "raw", static_cast(raw.xmag), time); + emit valueChanged(uasId, "mag y", "raw", static_cast(raw.ymag), time); + emit valueChanged(uasId, "mag z", "raw", static_cast(raw.zmag), time); } break; case MAVLINK_MSG_ID_ATTITUDE: @@ -1038,7 +1055,7 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc break; case MAV_STATE_EMERGENCY: uasState = tr("EMERGENCY"); - stateDescription = tr("EMERGENCY: Please land"); + stateDescription = tr("EMERGENCY: Land!"); break; case MAV_STATE_POWEROFF: uasState = tr("SHUTDOWN"); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index dfd9d11d4693a7e50fb41fbd2435dde1527e8725..603e55e3c279f63b383785847d10749535e4ab1d 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -89,9 +89,10 @@ public: double getRoll() const { return roll; } double getPitch() const { return pitch; } double getYaw() const { return yaw; } - + bool getSelected() const; friend class UASWaypointManager; + protected: //COMMENTS FOR TEST UNIT int uasId; ///< Unique system ID unsigned char type; ///< UAS type (from type enum) diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 514d596a24d7cdd6ced9da27aede6ed239186a55..77f3c00dcf35146f6126369d73aff1da0e84b82c 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -78,6 +78,8 @@ public: virtual double getPitch() const = 0; virtual double getYaw() const = 0; + virtual bool getSelected() const = 0; + /** @brief Get reference to the waypoint manager **/ virtual UASWaypointManager* getWaypointManager(void) = 0; @@ -419,6 +421,8 @@ signals: void heartbeatTimeout(unsigned int ms); /** @brief Name of system changed */ void nameChanged(QString newName); + /** @brief System has been selected as focused system */ + void systemSelected(bool selected); protected: diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 68ef921ea2f0eb79b3b4df6927baf5f6c3d15327..e2fd16f48a0f57a2c7bed000b6e48bef31a6019d 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -203,6 +203,7 @@ void UASManager::setActiveUAS(UASInterface* uas) activeUAS = uas; activeUASMutex.unlock(); + activeUAS->setSelected(); emit activeUASSet(uas); emit activeUASSet(uas->getUASID()); emit activeUASSetListIndex(systems.indexOf(uas)); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index eb0317b8344f949247dbd42407f4f511fc54f1be..e339a2e5d4a5428ae72b8f2822fd93c99b9aa8c5 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -138,9 +138,9 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { - qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << wp->autocontinue << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action; + qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action; Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action); - addWaypoint(lwp); + addWaypoint(lwp, false); //get next waypoint current_wp_id++; @@ -267,9 +267,17 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m void UASWaypointManager::notifyOfChange(Waypoint* wp) { - Q_UNUSED(wp); - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId(); + // If only one waypoint was changed, emit only WP signal + if (wp != NULL) + { + emit waypointChanged(uas.getUASID(), wp); + } + else + { + emit waypointListChanged(); + emit waypointListChanged(uas.getUASID()); + } } int UASWaypointManager::setCurrentWaypoint(quint16 seq) @@ -312,17 +320,20 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) return -1; } -void UASWaypointManager::addWaypoint(Waypoint *wp) +/** + * @param enforceFirstActive Enforces that the first waypoint is set as active + */ +void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive) { if (wp) { wp->setId(waypoints.size()); + if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true); waypoints.insert(waypoints.size(), wp); + connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*))); emit waypointListChanged(); emit waypointListChanged(uas.getUASID()); - - qDebug() << "ADDED WAYPOINT WITH ID:" << wp->getId(); } } @@ -510,6 +521,7 @@ void UASWaypointManager::writeWaypoints() { if (current_state == WP_IDLE) { + // Send clear all if count == 0 if (waypoints.count() > 0) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); @@ -522,7 +534,9 @@ void UASWaypointManager::writeWaypoints() current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; //clear local buffer - //TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers. + // Why not replace with waypoint_buffer.clear() ? + // because this will lead to memory leaks, the waypoint-structs + // have to be deleted, clear() would only delete the pointers. while(!waypoint_buffer.empty()) { delete waypoint_buffer.back(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index aa43dd564b2dd94e0aee62b9f51b302badcf5887..5e62476144ed26d03853b284d59e78caf028da7a 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -86,16 +86,6 @@ public: /** @name Waypoint list operations */ /*@{*/ const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the waypoint list. - int removeWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage - void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq - void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile - void loadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile - void notifyOfChange(Waypoint* wp); ///< Notifies manager to changes to a waypoint - void localAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly - int localRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage - void localMoveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq - void localSaveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile - void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list /*@}*/ @@ -122,7 +112,15 @@ private: public slots: void timeout(); ///< Called by the timer if a response times out. Handles send retries. - void addWaypoint(Waypoint *wp); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly + /** @name Waypoint list operations */ + /*@{*/ + void addWaypoint(Waypoint *wp, bool enforceFirstActive=true); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly + int removeWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq + void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile + void loadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile + void notifyOfChange(Waypoint* wp); ///< Notifies manager to changes to a waypoint + /*@}*/ signals: void waypointListChanged(void); ///< emits signal that the waypoint list has been changed diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 08c17ce5ebbdfa7771f5f2e9defcc296bef77bc5..5bd1487bcdf5058259976a8a7dc2b48966511212 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -38,7 +38,7 @@ #include "SlugsMAV.h" -#include "LogCompressor.h" +#include "LogCompressor.h"s MainWindow* MainWindow::instance() { @@ -72,13 +72,6 @@ MainWindow::MainWindow(QWidget *parent): { // Set this view as default view settings.setValue("CURRENT_VIEW", currentView); - - // Enable default tools - - // ENABLE UAS LIST - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,currentView), true); - // ENABLE COMMUNICATION CONSOLE - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE,currentView), true); } else { @@ -98,32 +91,9 @@ MainWindow::MainWindow(QWidget *parent): // Setup user interface ui.setupUi(this); - ui.actionNewCustomWidget->setEnabled(false); setVisible(false); - // Bind together the perspective actions - QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives); - perspectives->addAction(ui.actionEngineersView); - perspectives->addAction(ui.actionMavlinkView); - perspectives->addAction(ui.actionPilotsView); - perspectives->addAction(ui.actionOperatorsView); - perspectives->addAction(ui.actionUnconnectedView); - perspectives->setExclusive(true); - - // Mark the right one as selected - if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true); - if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); - if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); - if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); - if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true); - - // The pilot, engineer and operator view are not available on startup - // since they only make sense with a system connected. - ui.actionPilotsView->setEnabled(false); - ui.actionOperatorsView->setEnabled(false); - ui.actionEngineersView->setEnabled(false); - buildCommonWidgets(); connectCommonWidgets(); @@ -204,6 +174,11 @@ void MainWindow::setDefaultSettingsForAp() if (!settings.contains(centralKey)) { settings.setValue(centralKey,true); + + // ENABLE UAS LIST + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST, VIEW_UNCONNECTED), true); + // ENABLE COMMUNICATION CONSOLE + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE, VIEW_UNCONNECTED), true); } // OPERATOR VIEW DEFAULT @@ -223,6 +198,8 @@ void MainWindow::setDefaultSettingsForAp() if (!settings.contains(centralKey)) { settings.setValue(centralKey,true); + // Enable Parameter widget + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_PARAMETERS,VIEW_ENGINEER), true); } // MAVLINK VIEW DEFAULT @@ -237,6 +214,8 @@ void MainWindow::setDefaultSettingsForAp() if (!settings.contains(centralKey)) { settings.setValue(centralKey,true); + // Enable Flight display + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true); } } @@ -880,9 +859,11 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t // Key is built as follows: autopilot_type/section_menu/view/tool/section int apType; - apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? - UASManager::instance()->getActiveUAS()->getAutopilotType(): - -1; +// apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? +// UASManager::instance()->getActiveUAS()->getAutopilotType(): +// -1; + + apType = 1; return (QString::number(apType) + "_" + QString::number(SECTION_MENU) + "_" + @@ -1251,6 +1232,36 @@ void MainWindow::showInfoMessage(const QString& title, const QString& message) **/ void MainWindow::connectCommonActions() { + ui.actionNewCustomWidget->setEnabled(false); + + // Bind together the perspective actions + QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives); + perspectives->addAction(ui.actionEngineersView); + perspectives->addAction(ui.actionMavlinkView); + perspectives->addAction(ui.actionPilotsView); + perspectives->addAction(ui.actionOperatorsView); + perspectives->addAction(ui.actionUnconnectedView); + perspectives->setExclusive(true); + + // Mark the right one as selected + if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true); + if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); + if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); + if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); + if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true); + + // The pilot, engineer and operator view are not available on startup + // since they only make sense with a system connected. + ui.actionPilotsView->setEnabled(false); + ui.actionOperatorsView->setEnabled(false); + ui.actionEngineersView->setEnabled(false); + // The UAS actions are not enabled without connection to system + ui.actionLiftoff->setEnabled(false); + ui.actionLand->setEnabled(false); + ui.actionEmergency_Kill->setEnabled(false); + ui.actionEmergency_Land->setEnabled(false); + ui.actionShutdownMAV->setEnabled(false); + // Connect actions from ui connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); @@ -1263,7 +1274,7 @@ void MainWindow::connectCommonActions() connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); connect(ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS())); - + connect(ui.actionShutdownMAV, SIGNAL(triggered()), UASManager::instance(), SLOT(shutdownActiveUAS())); connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); // Views actions @@ -1424,6 +1435,12 @@ void MainWindow::UASCreated(UASInterface* uas) ui.actionPilotsView->setEnabled(true); ui.actionOperatorsView->setEnabled(true); ui.actionEngineersView->setEnabled(true); + // The UAS actions are not enabled without connection to system + ui.actionLiftoff->setEnabled(true); + ui.actionLand->setEnabled(true); + ui.actionEmergency_Kill->setEnabled(true); + ui.actionEmergency_Land->setEnabled(true); + ui.actionShutdownMAV->setEnabled(true); QIcon icon; // Set matching icon @@ -1629,6 +1646,7 @@ void MainWindow::clearView() { // Remove dock widget from main window removeDockWidget(dockWidget); + dockWidget->hide(); //dockWidget->setVisible(false); } } diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 85e8412f13c36da33604b7e93a6a358969609269..e50845e0b7444625bc31fabeea567caead410647 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -77,7 +77,7 @@ - false + true Select System @@ -85,7 +85,7 @@ - false + true Unmanned System @@ -93,6 +93,7 @@ false + @@ -413,6 +414,21 @@ Meta+U + + + + :/images/actions/system-log-out.svg:/images/actions/system-log-out.svg + + + Shutdown MAV + + + Shutdown the onboard computer - works not during flight + + + Shutdown the onboard computer - works not during flight + + diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index ff7bff7046867f20b38e1ce9dce33bc8ca5b8d3c..7d0913a182f43e27539bcb07dcae328f1aed98ef 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -36,7 +36,7 @@ MapWidget::MapWidget(QWidget *parent) : m_ui(new Ui::MapWidget) { m_ui->setupUi(this); - mc = new qmapcontrol::MapControl(QSize(320, 240)); + mc = new qmapcontrol::MapControl(this->size()); // VISUAL MAP STYLE QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }"); @@ -94,9 +94,9 @@ MapWidget::MapWidget(QWidget *parent) : // SET INITIAL POSITION AND ZOOM // Set default zoom level - mc->setZoom(17); + mc->setZoom(16); // Zurich, ETH - mc->setView(QPointF(8.548056,47.376389)); + mc->setView(QPointF(8.548056,47.376889)); // Veracruz Mexico //mc->setView(QPointF(-96.105208,19.138955)); @@ -157,8 +157,8 @@ MapWidget::MapWidget(QWidget *parent) : zoomout->setStyleSheet(buttonStyle); createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this); createPath->setStyleSheet(buttonStyle); - clearTracking = new QPushButton(QIcon(""), "", this); - clearTracking->setStyleSheet(buttonStyle); +// clearTracking = new QPushButton(QIcon(""), "", this); +// clearTracking->setStyleSheet(buttonStyle); followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); followgps->setStyleSheet(buttonStyle); QPushButton* goToButton = new QPushButton(QIcon(""), "T", this); @@ -167,7 +167,7 @@ MapWidget::MapWidget(QWidget *parent) : zoomin->setMaximumWidth(30); zoomout->setMaximumWidth(30); createPath->setMaximumWidth(30); - clearTracking->setMaximumWidth(30); +// clearTracking->setMaximumWidth(30); followgps->setMaximumWidth(30); goToButton->setMaximumWidth(30); @@ -185,7 +185,7 @@ MapWidget::MapWidget(QWidget *parent) : innerlayout->addWidget(zoomout, 1, 0); innerlayout->addWidget(followgps, 2, 0); innerlayout->addWidget(createPath, 3, 0); - innerlayout->addWidget(clearTracking, 4, 0); + //innerlayout->addWidget(clearTracking, 4, 0); // Add spacers to compress buttons on the top left innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7); @@ -254,26 +254,26 @@ MapWidget::MapWidget(QWidget *parent) : void MapWidget::goTo() { bool ok; - QString text = QInputDialog::getText(this, tr("Please enter coordinates"), - tr("Coordinates (Lat,Lon):"), QLineEdit::Normal, - QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok); - if (ok && !text.isEmpty()) - { - QStringList split = text.split(","); - if (split.length() == 2) - { - bool convert; - double latitude = split.first().toDouble(&convert); - ok &= convert; - double longitude = split.last().toDouble(&convert); - ok &= convert; - - if (ok) - { - mc->setView(QPointF(latitude, longitude)); - } - } - } + QString text = QInputDialog::getText(this, tr("Please enter coordinates"), + tr("Coordinates (Lat,Lon):"), QLineEdit::Normal, + QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok); + if (ok && !text.isEmpty()) + { + QStringList split = text.split(","); + if (split.length() == 2) + { + bool convert; + double latitude = split.first().toDouble(&convert); + ok &= convert; + double longitude = split.last().toDouble(&convert); + ok &= convert; + + if (ok) + { + mc->setView(QPointF(latitude, longitude)); + } + } + } } @@ -290,7 +290,7 @@ void MapWidget::mapproviderSelected(QAction* action) l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); // yahooActionOverlay->setEnabled(false); overlay->setVisible(false); @@ -306,7 +306,7 @@ void MapWidget::mapproviderSelected(QAction* action) l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); // yahooActionOverlay->setEnabled(false); overlay->setVisible(false); @@ -321,7 +321,7 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1"); l->setMapAdapter(mapadapter); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); // yahooActionOverlay->setEnabled(true); } @@ -333,7 +333,7 @@ void MapWidget::mapproviderSelected(QAction* action) l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); // yahooActionOverlay->setEnabled(false); overlay->setVisible(false); @@ -347,7 +347,7 @@ void MapWidget::mapproviderSelected(QAction* action) l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); // yahooActionOverlay->setEnabled(false); overlay->setVisible(false); @@ -411,12 +411,13 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina if (mav) { - mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y())); + mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y(), 0.0f, 0.0f, true)); } else { str = QString("%1").arg(waypointPath->numberOfPoints()); tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle); + wpIcons.append(tempCirclePoint); mc->layer("Waypoints")->addGeometry(tempCirclePoint); @@ -425,7 +426,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina waypointPath->addPoint(tempPoint); // Refresh the screen - mc->updateRequest(tempPoint->boundingBox().toRect()); + if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); } // emit signal mouse was clicked @@ -435,21 +436,25 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina void MapWidget::updateWaypoint(int uas, Waypoint* wp) { - //qDebug() << "UPDATING WP" << wp->getId() << __FILE__ << __LINE__; + updateWaypoint(uas, wp, true); +} + +void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) +{ + qDebug() << "UPDATING WP" << wp->getId() << wp << __FILE__ << __LINE__; if (uas == this->mav->getUASID()) { int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp); + if (wpindex == -1) return; // Create waypoint name - QString str = QString("%1").arg(wpindex); + //QString str = QString("%1").arg(wpindex); // Check if wp exists yet - if (!(wps.count() > wpindex)) + if (!(wpIcons.count() > wpindex)) { QPointF coordinate; coordinate.setX(wp->getX()); coordinate.setY(wp->getY()); createWaypointGraphAtMap(wpindex, coordinate); - - qDebug() << "Waypoint Index did not contain" << str; } else { @@ -478,6 +483,10 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp) { linesegment = waypointPath->points().at(wpindex); } + else + { + waypointPath->addPoint(waypoint); + } if (linesegment) { @@ -486,7 +495,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp) //point2Find = dynamic_cast (mc->layer("Waypoints")->get_Geometry(wpindex)); //point2Find->setCoordinate(coordinate); - mc->updateRequest(waypoint->boundingBox().toRect()); + if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect()); } } } @@ -529,7 +538,7 @@ void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate) qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude(); // Refresh the screen - mc->updateRequest(tempPoint->boundingBox().toRect()); + if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); } //// // emit signal mouse was clicked @@ -562,7 +571,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) waypointIsDrag = true; // Refresh the screen - mc->updateRequest(geom->boundingBox().toRect()); + if (isVisible()) mc->updateRequest(geom->boundingBox().toRect()); int temp = 0; @@ -570,40 +579,35 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) bool wpIndexOk; int index = geom->name().toInt(&wpIndexOk); - qmapcontrol::Point* point2Find; - point2Find = wps.at(geom->name().toInt()); + Waypoint2DIcon* point2Find = dynamic_cast (geom); - if (point2Find) + if (wpIndexOk && point2Find && wps.count() > index) { + // Update visual point2Find->setCoordinate(coordinate); + waypointPath->points().at(index)->setCoordinate(coordinate); + if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect()); - point2Find = dynamic_cast (geom); - if (point2Find) + // Update waypoint data storage + if (mav) { - point2Find->setCoordinate(coordinate); + QVector wps = mav->getWaypointManager()->getWaypointList(); - if (wpIndexOk) + if (wps.size() > index) { - mc->updateRequest(point2Find->boundingBox().toRect()); - if (mav) - { - QVector wps = mav->getWaypointManager()->getWaypointList(); - - if (wps.size() > index) - { - wps.at(index)->setX(coordinate.x()); - wps.at(index)->setY(coordinate.y()); - mav->getWaypointManager()->notifyOfChange(wps.at(index)); - } - } + wps.at(index)->setX(coordinate.x()); + wps.at(index)->setY(coordinate.y()); + mav->getWaypointManager()->notifyOfChange(wps.at(index)); } - // qDebug() << geom->name(); - temp = geom->get_myIndex(); - //qDebug() << temp; - emit sendGeometryEndDrag(coordinate,temp); } + + // qDebug() << geom->name(); + temp = geom->get_myIndex(); + //qDebug() << temp; + emit sendGeometryEndDrag(coordinate,temp); } + waypointIsDrag = false; } void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) @@ -632,6 +636,7 @@ MapWidget::~MapWidget() void MapWidget::addUAS(UASInterface* uas) { connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); //connect(uas->getWaypointManager(), SIGNAL(waypointListChanged()), this, SLOT(redoWaypoints())); } @@ -641,6 +646,9 @@ void MapWidget::updateWaypointList(int uas) UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); if (uasInstance) { + // Get update rect of old content + QRect updateRect = waypointPath->boundingBox().toRect(); + QVector wpList = uasInstance->getWaypointManager()->getWaypointList(); // Clear if necessary @@ -653,28 +661,25 @@ void MapWidget::updateWaypointList(int uas) // Load all existing waypoints into map view foreach (Waypoint* wp, wpList) { - updateWaypoint(mav->getUASID(), wp); + // Block updates, since we update everything in the next step + updateWaypoint(mav->getUASID(), wp, false); } // Delete now unused wps - if (wps.count() > wpList.count()) + if (waypointPath->points().count() > wpList.count()) { - mc->layer("Waypoints")->removeGeometry(waypointPath); - for (int i = wpList.count(); i < wps.count(); ++i) + int overSize = waypointPath->points().count() - wpList.count(); + for (int i = 0; i < overSize; ++i) { - QRect updateRect = wps.at(i)->boundingBox().toRect(); - wps.removeAt(i); - mc->layer("Waypoints")->removeGeometry(wpIcons.at(i)); - waypointPath->points().removeAt(i); - //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp)); - - mc->updateRequest(updateRect); + wps.removeLast(); + mc->layer("Waypoints")->removeGeometry(wpIcons.last()); + wpIcons.removeLast(); + waypointPath->points().removeLast(); } - mc->layer("Waypoints")->addGeometry(waypointPath); } - // Clear and rebuild linestring - + // Update view + if (isVisible()) mc->updateRequest(updateRect); } } @@ -687,7 +692,7 @@ void MapWidget::redoWaypoints(int uas) // Get waypoint list for this MAV // Clear all waypoints - clearWaypoints(); + //clearWaypoints(); // Re-add the updated waypoints // } @@ -700,34 +705,63 @@ void MapWidget::activeUASSet(UASInterface* uas) // Disconnect old MAV if (mav) { - // clear path create on the map + // Disconnect the waypoint manager / data storage from the UI disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - // add Waypoint widget in the WaypointList widget when mouse clicked + disconnect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); - - - // it notifies that a waypoint global goes to do create and a map graphic too - //connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF))); } if (uas) { mav = uas; - QColor color = mav->getColor().lighter(100); - color.setAlphaF(0.6); + QColor color = mav->getColor(); + color.setAlphaF(0.9); QPen* pen = new QPen(color); - pen->setWidth(2.0); + pen->setWidth(3.0); mavPens.insert(mav->getUASID(), pen); // FIXME Remove after refactoring waypointPath->setPen(pen); - // Delete all waypoints and start fresh - redoWaypoints(); + // Delete all waypoints and add waypoint from new system + //redoWaypoints(); + updateWaypointList(uas->getUASID()); - // clear path create on the map + // Connect the waypoint manager / data storage to the UI connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - // add Waypoint widget in the WaypointList widget when mouse clicked + connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); + + updateSelectedSystem(mav->getUASID()); + mc->updateRequest(waypointPath->boundingBox().toRect()); + } +} + +void MapWidget::updateSelectedSystem(int uas) +{ + foreach (qmapcontrol::Point* p, uasIcons.values()) + { + MAV2DIcon* icon = dynamic_cast(p); + if (icon) + { + // Set as selected if ids match + icon->setSelectedUAS((icon->getUASId() == uas)); + } + } +} + +void MapWidget::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec) +{ + Q_UNUSED(roll); + Q_UNUSED(pitch); + Q_UNUSED(usec); + + if (uas) + { + MAV2DIcon* icon = dynamic_cast(uasIcons.value(uas->getUASID(), NULL)); + if (icon) + { + icon->setYaw(yaw); + } } } @@ -765,58 +799,59 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, // Icon //QPen* pointpen = new QPen(uasColor); qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__; - //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID())); - p = new Waypoint2DIcon(lat, lon, 20, QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle); + p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle); uasIcons.insert(uas->getUASID(), p); mc->layer("Waypoints")->addGeometry(p); // Line // A QPen also can use transparency - QList points; - points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); - QPen* linepen = new QPen(uasColor.darker()); - linepen->setWidth(2); + // QList points; + // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); + // QPen* linepen = new QPen(uasColor.darker()); + // linepen->setWidth(2); - // Create tracking line string - qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); - uasTrails.insert(uas->getUASID(), ls); + // // Create tracking line string + // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); + // uasTrails.insert(uas->getUASID(), ls); - // Add the LineString to the layer - mc->layer("Waypoints")->addGeometry(ls); + // // Add the LineString to the layer + // mc->layer("Waypoints")->addGeometry(ls); } else { -// p = dynamic_cast(uasIcons.value(uas->getUASID())); -// if (p) -// { - p = uasIcons.value(uas->getUASID()); - p->setCoordinate(QPointF(lat, lon)); - //p->setYaw(uas->getYaw()); -// } + // p = dynamic_cast(uasIcons.value(uas->getUASID())); + // if (p) + // { + p = uasIcons.value(uas->getUASID()); + p->setCoordinate(QPointF(lat, lon)); + //p->setYaw(uas->getYaw()); + // } // Extend trail - uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y())); + // uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y())); } -mc->updateRequest(p->boundingBox().toRect()); - - //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); + if (isVisible()) mc->updateRequest(p->boundingBox().toRect()); + //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); - // Limit the position update rate - quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastUpdate > 120) + if (this->mav && uas->getUASID() == this->mav->getUASID()) { - lastUpdate = currTime; - // Sets the view to the interesting area - if (followgps->isChecked()) + // Limit the position update rate + quint64 currTime = MG::TIME::getGroundTimeNow(); + if (currTime - lastUpdate > 120) { - updatePosition(0, lat, lon); - } - else - { - // Refresh the screen - //mc->updateRequestNew(); + lastUpdate = currTime; + // Sets the view to the interesting area + if (followgps->isChecked()) + { + updatePosition(0, lat, lon); + } + else + { + // Refresh the screen + //if (isVisible()) mc->updateRequestNew(); + } } } } @@ -908,16 +943,28 @@ void MapWidget::changeEvent(QEvent *e) void MapWidget::clearWaypoints(int uas) { + Q_UNUSED(uas); // Clear the previous WP track //mc->layer("Waypoints")->clearGeometries(); wps.clear(); + foreach (Point* p, wpIcons) + { + mc->layer("Waypoints")->removeGeometry(p); + } + wpIcons.clear(); + + // Get bounding box of this object BEFORE deleting the content + QRect box = waypointPath->boundingBox().toRect(); + + // Delete the content waypointPath->points().clear(); + //delete waypointPath; //waypointPath = new - mc->layer("Waypoints")->addGeometry(waypointPath); + //mc->layer("Waypoints")->addGeometry(waypointPath); //wpIndex.clear(); - mc->updateRequestNew();//(waypointPath->boundingBox().toRect()); + if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect()); if(createPath->isChecked()) { @@ -929,6 +976,7 @@ void MapWidget::clearWaypoints(int uas) void MapWidget::clearPath(int uas) { + Q_UNUSED(uas); mc->layer("Tracking")->clearGeometries(); foreach (qmapcontrol::LineString* ls, uasTrails) { @@ -938,11 +986,13 @@ void MapWidget::clearPath(int uas) mc->layer("Tracking")->addGeometry(lsNew); } // FIXME update this with update request only for bounding box of trails - mc->updateRequestNew();//(QRect(0, 0, width(), height())); + if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height())); } void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) { + Q_UNUSED(dir); + Q_UNUSED(bearing); // FIXME Mariano //camPoints.clear(); QPointF currentPos = mc->currentCoordinate(); @@ -973,14 +1023,14 @@ void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) mc->layer("Camera")->addGeometry(camBorder); // mc->layer("Camera")->addGeometry(camLine); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); } else { //clear camera borders mc->layer("Camera")->clearGeometries(); - mc->updateRequestNew(); + if (isVisible()) mc->updateRequestNew(); } diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index 91d2b6d2b2fd66a121892d31f884832626b26223..9db98ab77c77bb1a9077382920fea5389467f802 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -68,6 +68,10 @@ public: public slots: void addUAS(UASInterface* uas); void activeUASSet(UASInterface* uas); + /** @brief Update the selected system */ + void updateSelectedSystem(int uas); + /** @brief Update the attitude */ + void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec); void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); void updatePosition(float time, double lat, double lon); void updateCameraPosition(double distance, double bearing, QString dir); @@ -85,6 +89,7 @@ public slots: /** @brief Update waypoint */ void updateWaypoint(int uas, Waypoint* wp); + void updateWaypoint(int uas, Waypoint* wp, bool updateView); void drawBorderCamAtMap(bool status); /** @brief Bring up dialog to go to a specific location */ diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 7c4fc341b2c159f9d1c9d6b8ab4e100f35da0ec9..67f023eba97786ba0808643e68b6cf56f17d0504 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -8,17 +8,19 @@ #include "ui_QGCMAVLinkLogPlayer.h" QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *parent) : - QWidget(parent), - lineCounter(0), - totalLines(0), - startTime(0), - endTime(0), - currentStartTime(0), - accelerationFactor(1.0f), - mavlink(mavlink), - logLink(NULL), - loopCounter(0), - ui(new Ui::QGCMAVLinkLogPlayer) + QWidget(parent), + lineCounter(0), + totalLines(0), + startTime(0), + endTime(0), + currentStartTime(0), + accelerationFactor(1.0f), + mavlink(mavlink), + logLink(NULL), + loopCounter(0), + mavlinkLogFormat(true), + binaryBaudRate(57600), + ui(new Ui::QGCMAVLinkLogPlayer) { ui->setupUi(this); ui->gridLayout->setAlignment(Qt::AlignTop); @@ -62,7 +64,20 @@ void QGCMAVLinkLogPlayer::play() logLink = new MAVLinkSimulationLink(""); // Start timer - loopTimer.start(1); + if (mavlinkLogFormat) + { + loopTimer.start(1); + } + else + { + // Read len bytes at a time + int len = 100; + // Calculate the number of times to read 100 bytes per second + // to guarantee the baud rate, then divide 1000 by the number of read + // operations to obtain the interval in milliseconds + int interval = 1000 / ((binaryBaudRate / 10) / len); + loopTimer.start(interval*accelerationFactor); + } } else { @@ -96,6 +111,7 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex) // Reset only for valid values if (packetIndex >= 0 && packetIndex <= logFile.size() - (packetLen+timeLen)) { + bool result = true; pause(); loopCounter = 0; @@ -124,7 +140,7 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex) void QGCMAVLinkLogPlayer::selectLogFile() { - QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink);;")); + QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink or Binary Logfile (*.mavlink; *.bin);;")); loadLogFile(fileName); } @@ -146,6 +162,19 @@ void QGCMAVLinkLogPlayer::setAccelerationFactorInt(int factor) accelerationFactor = 1+(f/2.0f); } + // Update timer interval + if (!mavlinkLogFormat) + { + // Read len bytes at a time + int len = 100; + // Calculate the number of times to read 100 bytes per second + // to guarantee the baud rate, then divide 1000 by the number of read + // operations to obtain the interval in milliseconds + int interval = 1000 / ((binaryBaudRate / 10) / len); + loopTimer.stop(); + loopTimer.start(interval/accelerationFactor); + } + //qDebug() << "FACTOR:" << accelerationFactor; ui->speedLabel->setText(tr("Speed: %1X").arg(accelerationFactor, 5, 'f', 2, '0')); @@ -180,30 +209,64 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file) startTime = 0; ui->logFileNameLabel->setText(tr("%1").arg(logFileInfo.baseName())); - // Get the time interval from the logfile - QByteArray timestamp = logFile.read(timeLen); + // Select if binary or MAVLink log format is used + mavlinkLogFormat = file.endsWith(".mavlink"); - // First timestamp - quint64 starttime = *((quint64*)(timestamp.constData())); + if (mavlinkLogFormat) + { + // Get the time interval from the logfile + QByteArray timestamp = logFile.read(timeLen); - // Last timestamp - logFile.seek(logFile.size()-packetLen-timeLen); - QByteArray timestamp2 = logFile.read(timeLen); - quint64 endtime = *((quint64*)(timestamp2.constData())); - // Reset everything - logFile.reset(); + // First timestamp + quint64 starttime = *((quint64*)(timestamp.constData())); - qDebug() << "Starttime:" << starttime << "End:" << endtime; + // Last timestamp + logFile.seek(logFile.size()-packetLen-timeLen); + QByteArray timestamp2 = logFile.read(timeLen); + quint64 endtime = *((quint64*)(timestamp2.constData())); + // Reset everything + logFile.reset(); + + qDebug() << "Starttime:" << starttime << "End:" << endtime; - // WARNING: Order matters in this computation - int seconds = (endtime - starttime)/1000000; - int minutes = seconds / 60; - int hours = minutes / 60; - seconds -= 60*minutes; - minutes -= 60*hours; + // WARNING: Order matters in this computation + int seconds = (endtime - starttime)/1000000; + int minutes = seconds / 60; + int hours = minutes / 60; + seconds -= 60*minutes; + minutes -= 60*hours; - QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2); - ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel)); + QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2); + ui->logStatsLabel->setText(tr("%2 MB, %3 packets, %4").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(logFileInfo.size()/(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))).arg(timelabel)); + } + else + { + // Load in binary mode + + // Set baud rate if any present + QStringList parts = logFileInfo.baseName().split("_"); + + if (parts.count() > 1) + { + bool ok; + int rate = parts.last().toInt(&ok); + // 9600 baud to 100 MBit + if (ok && (rate > 9600 && rate < 100000000)) + { + // Accept this as valid baudrate + binaryBaudRate = rate; + } + } + + int seconds = logFileInfo.size() / (binaryBaudRate / 10); + int minutes = seconds / 60; + int hours = minutes / 60; + seconds -= 60*minutes; + minutes -= 60*hours; + + QString timelabel = tr("%1h:%2m:%3s").arg(hours, 2).arg(minutes, 2).arg(seconds, 2); + ui->logStatsLabel->setText(tr("%2 MB, %4 at %5 KB/s").arg(logFileInfo.size()/1000000.0f, 0, 'f', 2).arg(timelabel).arg(binaryBaudRate/10.0f/1024.0f, 0, 'f', 2)); + } } } @@ -213,17 +276,19 @@ void QGCMAVLinkLogPlayer::loadLogFile(const QString& file) void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue) { loopTimer.stop(); - // Set the logfile to the correct percentage and - // align to the timestamp values - int packetCount = logFile.size() / (packetLen + timeLen); - int packetIndex = (packetCount - 1) * (slidervalue / (double)(ui->positionSlider->maximum() - ui->positionSlider->minimum())); + // Set the logfile to the correct percentage and + // align to the timestamp values + int packetCount = logFile.size() / (packetLen + timeLen); + int packetIndex = (packetCount - 1) * (slidervalue / (double)(ui->positionSlider->maximum() - ui->positionSlider->minimum())); - // Do only accept valid jumps - if (reset(packetIndex)) - { - ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex)); - //qDebug() << "SET POSITION TO PACKET:" << packetIndex; - } + // Do only accept valid jumps + if (reset(packetIndex)) + { + if (mavlinkLogFormat) + { + ui->logStatsLabel->setText(tr("Jumped to packet %1").arg(packetIndex)); + } + } } /** @@ -236,115 +301,137 @@ void QGCMAVLinkLogPlayer::jumpToSliderVal(int slidervalue) */ void QGCMAVLinkLogPlayer::logLoop() { - bool ok; - - // First check initialization - if (startTime == 0) + if (mavlinkLogFormat) { - QByteArray startBytes = logFile.read(timeLen); + bool ok; - // Check if the correct number of bytes could be read - if (startBytes.length() != timeLen) + // First check initialization + if (startTime == 0) { - ui->logStatsLabel->setText(tr("Error reading first %1 bytes").arg(timeLen)); - MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Error reading first %1 bytes from logfile. Got %2 instead of %1 bytes. Is the logfile readable?").arg(timeLen).arg(startBytes.length())); - reset(); - return; + QByteArray startBytes = logFile.read(timeLen); + + // Check if the correct number of bytes could be read + if (startBytes.length() != timeLen) + { + ui->logStatsLabel->setText(tr("Error reading first %1 bytes").arg(timeLen)); + MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Error reading first %1 bytes from logfile. Got %2 instead of %1 bytes. Is the logfile readable?").arg(timeLen).arg(startBytes.length())); + reset(); + return; + } + + // Convert data to timestamp + startTime = *((quint64*)(startBytes.constData())); + currentStartTime = QGC::groundTimeUsecs(); + ok = true; + + //qDebug() << "START TIME: " << startTime; + + // Check if these bytes could be correctly decoded + if (!ok) + { + ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting.")); + MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Could not load initial timestamp from file %1. Is the file corrupted?").arg(logFile.fileName())); + reset(); + return; + } } - // Convert data to timestamp - startTime = *((quint64*)(startBytes.constData())); - currentStartTime = QGC::groundTimeUsecs(); - ok = true; - //qDebug() << "START TIME: " << startTime; + // Initialization seems fine, load next chunk + QByteArray chunk = logFile.read(timeLen+packetLen); + QByteArray packet = chunk.mid(0, packetLen); - // Check if these bytes could be correctly decoded - if (!ok) + // Emit this packet + emit bytesReady(logLink, packet); + + // Check if reached end of file before reading next timestamp + if (chunk.length() < timeLen+packetLen || logFile.atEnd()) { - ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting.")); - MainWindow::instance()->showCriticalMessage(tr("Failed loading MAVLink Logfile"), tr("Could not load initial timestamp from file %1. Is the file corrupted?").arg(logFile.fileName())); + // Reached end of file reset(); + + QString status = tr("Reached end of MAVLink log file."); + ui->logStatsLabel->setText(status); + MainWindow::instance()->showStatusMessage(status); return; } - } + // End of file not reached, read next timestamp + QByteArray rawTime = chunk.mid(packetLen); + + // This is the timestamp of the next packet + quint64 time = *((quint64*)(rawTime.constData())); + ok = true; + if (!ok) + { + // Convert it to 64bit number + QString status = tr("Time conversion error during log replay. Continuing.."); + ui->logStatsLabel->setText(status); + MainWindow::instance()->showStatusMessage(status); + } + else + { + // Normal processing, passed all checks + // start timer to match time offset between + // this and next packet - // Initialization seems fine, load next chunk - QByteArray chunk = logFile.read(timeLen+packetLen); - QByteArray packet = chunk.mid(0, packetLen); - // Emit this packet - emit bytesReady(logLink, packet); + // Offset in ms + qint64 timediff = (time - startTime)/accelerationFactor; - // Check if reached end of file before reading next timestamp - if (chunk.length() < timeLen+packetLen || logFile.atEnd()) - { - // Reached end of file - reset(); + // Immediately load any data within + // a 3 ms interval - QString status = tr("Reached end of MAVLink log file."); - ui->logStatsLabel->setText(status); - MainWindow::instance()->showStatusMessage(status); - return; - } + int nextExecutionTime = (((qint64)currentStartTime + (qint64)timediff) - (qint64)QGC::groundTimeUsecs())/1000; - // End of file not reached, read next timestamp - QByteArray rawTime = chunk.mid(packetLen); + //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime; - // This is the timestamp of the next packet - quint64 time = *((quint64*)(rawTime.constData())); - ok = true; - if (!ok) - { - // Convert it to 64bit number - QString status = tr("Time conversion error during log replay. Continuing.."); - ui->logStatsLabel->setText(status); - MainWindow::instance()->showStatusMessage(status); + if (nextExecutionTime < 2) + { + logLoop(); + } + else + { + loopTimer.start(nextExecutionTime); + } + } } else { - // Normal processing, passed all checks - // start timer to match time offset between - // this and next packet - - - // Offset in ms - qint64 timediff = (time - startTime)/accelerationFactor; + // Binary format - read at fixed rate + const int len = 100; + QByteArray chunk = logFile.read(len); - // Immediately load any data within - // a 3 ms interval + // Emit this packet + emit bytesReady(logLink, chunk); - int nextExecutionTime = (((qint64)currentStartTime + (qint64)timediff) - (qint64)QGC::groundTimeUsecs())/1000; - - //qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime; - - if (nextExecutionTime < 2) + // Check if reached end of file before reading next timestamp + if (chunk.length() < len || logFile.atEnd()) { - logLoop(); - } - else - { - loopTimer.start(nextExecutionTime); - } -// loopTimer.start(20); + // Reached end of file + reset(); - if (loopCounter % 40 == 0) - { - QFileInfo logFileInfo(logFile); - int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast(logFileInfo.size())); - //qDebug() << "Progress:" << progress; - ui->positionSlider->blockSignals(true); - ui->positionSlider->setValue(progress); - ui->positionSlider->blockSignals(false); + QString status = tr("Reached end of binary log file."); + ui->logStatsLabel->setText(status); + MainWindow::instance()->showStatusMessage(status); + return; } - loopCounter++; - // Ui update: Only every 20 messages - // to prevent flickering and high CPU load + } + // Ui update: Only every 20 messages + // to prevent flickering and high CPU load - // Update status label - // Update progress bar + // Update status label + // Update progress bar + if (loopCounter % 40 == 0) + { + QFileInfo logFileInfo(logFile); + int progress = (ui->positionSlider->maximum()-ui->positionSlider->minimum())*(logFile.pos()/static_cast(logFileInfo.size())); + //qDebug() << "Progress:" << progress; + ui->positionSlider->blockSignals(true); + ui->positionSlider->setValue(progress); + ui->positionSlider->blockSignals(false); } + loopCounter++; } void QGCMAVLinkLogPlayer::changeEvent(QEvent *e) diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index 1ec0603cbf45aed830b9450f97f6556568ec00c5..58a459c3b99e7b8c380699f6730d6b9a550b0312 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -61,6 +61,8 @@ protected: QFile logFile; QTimer loopTimer; int loopCounter; + bool mavlinkLogFormat; + int binaryBaudRate; static const int packetLen = MAVLINK_MAX_PACKET_LEN; static const int timeLen = sizeof(quint64); void changeEvent(QEvent *e); diff --git a/src/ui/UASView.ui b/src/ui/UASView.ui index eecc77b37cd63d72c312240d3f096d2152a27a07..78e602923ad91da99e439c07c79a13dc42a113b7 100644 --- a/src/ui/UASView.ui +++ b/src/ui/UASView.ui @@ -146,10 +146,20 @@ QPushButton:pressed#killButton { } -QProgressBar { +QProgressBar:horizontal { + border: 1px solid #4A4A4F; + border-radius: 4px; + text-align: center; + padding: 2px; + color: #DDDDDF; + background-color: #111118; +} + +QProgressBar:vertical { border: 1px solid #4A4A4F; border-radius: 4px; text-align: center; + font-size: 7px; padding: 2px; color: #DDDDDF; background-color: #111118; @@ -159,12 +169,16 @@ QProgressBar:horizontal { height: 10px; } -QProgressBar QLabel { - font-size: 8px; +QProgressBar:horizontal QLabel { + font-size: 9px; +} + +QProgressBar:vertical QLabel { + font-size: 7px; } QProgressBar:vertical { - width: 12px; + width: 14px; } QProgressBar::chunk { @@ -231,7 +245,7 @@ QMenu::separator { - + 4 @@ -281,9 +295,12 @@ QMenu::separator { Qt::Horizontal + + QSizePolicy::MinimumExpanding + - 6 + 4 88 @@ -327,7 +344,7 @@ QMenu::separator { - Waiting for first update.. + @@ -466,7 +483,7 @@ QMenu::separator { - UNINIT + @@ -481,14 +498,14 @@ QMenu::separator { - WPX + --- Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - + @@ -515,33 +532,6 @@ QMenu::separator { - - - - - 0 - 12 - - - - - 16777215 - 12 - - - - - -1 - 50 - false - false - - - - 00 00 00 N 00 00 00 E - - - @@ -556,7 +546,7 @@ QMenu::separator { - Unknown status + diff --git a/src/ui/WaypointGlobalView.cc b/src/ui/WaypointGlobalView.cc deleted file mode 100644 index e5e468ff30d82fa87d60bbf526165a964cb11da3..0000000000000000000000000000000000000000 --- a/src/ui/WaypointGlobalView.cc +++ /dev/null @@ -1,467 +0,0 @@ -#include "WaypointGlobalView.h" -#include "ui_WaypointGlobalView.h" - -#include - -WaypointGlobalView::WaypointGlobalView(Waypoint* wp,QWidget *parent) : - QWidget(parent), - ui(new Ui::WaypointGlobalView) -{ - ui->setupUi(this); - this->wp = wp; - - ui->m_orbitalSpinBox->hide(); - - // set type - wp->setType(Waypoint::GLOBAL); - - // Read values and set user interface - updateValues(); - - connect(ui->m_orbitalSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double))); - connect(ui->m_heigthSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); - - //for spinBox Latitude - connect(ui->m_latitudGrados_spinBox, SIGNAL(valueChanged(int)), this, SLOT(updateLatitudeWP(int))); - connect(ui->m_latitudMinutos_spinBox, SIGNAL(valueChanged(double)), this, SLOT(updateLatitudeMinuteWP(double))); - connect(ui->m_dirLatitudeN_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLatitudeWP())); - connect(ui->m_dirLatitudeS_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLatitudeWP())); - - //for spinBox Longitude - connect(ui->m_longitudGrados_spinBox, SIGNAL(valueChanged(int)), this, SLOT(updateLongitudeWP(int))); - connect(ui->m_longitudMinutos_spinBox, SIGNAL(valueChanged(double)), this, SLOT(updateLongitudeMinuteWP(double))); - connect(ui->m_dirLongitudeW_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLongitudeWP())); - connect(ui->m_dirLongitudeE_radioButton, SIGNAL(clicked()), this, SLOT(changeDirectionLongitudeWP())); - - - - connect(ui->m_orbitCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeOrbitalState(int))); - - - - - -// connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); -// connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); -// connect(m_ui->zSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); - -// //hidden degree to radian conversion of the yaw angle -// connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYaw(int))); -// connect(this, SIGNAL(setYaw(double)), wp, SLOT(setYaw(double))); - -// connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp())); -// connect(m_ui->downButton, SIGNAL(clicked()), this, SLOT(moveDown())); -// connect(m_ui->removeButton, SIGNAL(clicked()), this, SLOT(remove())); - -// connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int))); -// connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int))); - -// -// connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int))); -} - -WaypointGlobalView::~WaypointGlobalView() -{ - delete ui; -} - -void WaypointGlobalView::updateValues() -{ - - int gradoLat, gradoLon; - float minLat, minLon; - QString dirLat, dirLon; - - getLatitudeGradoMin(wp->getY(), &gradoLat, &minLat, &dirLat); - getLongitudGradoMin(wp->getX(), &gradoLon, &minLon, &dirLon); - - //latitude on spinBox - ui->m_latitudGrados_spinBox->setValue(gradoLat); - ui->m_latitudMinutos_spinBox->setValue(minLat); - if(dirLat == "N") - { - ui->m_dirLatitudeN_radioButton->setChecked(true); - ui->m_dirLatitudeS_radioButton->setChecked(false); - } - else - { - ui->m_dirLatitudeS_radioButton->setChecked(true); - ui->m_dirLatitudeN_radioButton->setChecked(false); - - } - - //longitude on spinBox - ui->m_longitudGrados_spinBox->setValue(gradoLon); - ui->m_longitudMinutos_spinBox->setValue(minLon); - if(dirLon == "W") - { - ui->m_dirLongitudeW_radioButton->setChecked(true); - ui->m_dirLongitudeE_radioButton->setChecked(false); - } - else - { - ui->m_dirLongitudeE_radioButton->setChecked(true); - ui->m_dirLongitudeW_radioButton->setChecked(false); - - } - - ui->idWP_label->setText(QString("WP-%1").arg(wp->getId())); - - -} - -void WaypointGlobalView::changeEvent(QEvent *e) -{ - switch (e->type()) { - case QEvent::LanguageChange: - ui->retranslateUi(this); - break; - default: - break; - } -} - -void WaypointGlobalView::remove() -{ - emit removeWaypoint(wp); - delete this; -} - -QString WaypointGlobalView::getLatitudString(float latitud) -{ - QString tempNS =""; - QString stringLatitudTemp = ""; - - float minutos = 0; - float grados = 0; - float entero = 0; - float dec = 0; - - if (latitud<0){tempNS="S"; latitud = latitud * -1;} - else {tempNS="N";} - - if(latitud< 90 || latitud > -90) - { - dec = latitud - (entero = ::floor(latitud));; - minutos = dec * 60; - grados = entero; - if(grados < 0) grados = grados * (-1); - if(minutos < 0) minutos = minutos * (-1); - - stringLatitudTemp = QString::number(grados)+ " ° "+ QString::number(minutos)+"' "+ tempNS; - - return stringLatitudTemp; - } - else - { - stringLatitudTemp = "erroneous latitude"; - return stringLatitudTemp; - } - -} - -QString WaypointGlobalView::getLongitudString(float longitud) -{ - QString tempEW =""; - QString stringLongitudTemp = ""; - - float minutos = 0; - float grados = 0; - float entero = 0; - float dec = 0; - - if (longitud<0){tempEW="W"; longitud = longitud * -1;} - else {tempEW="E";} - - if(longitud<180 || longitud > -180) - { - dec = longitud - (entero = ::floor(longitud));; - minutos = dec * 60; - grados = entero; - if(grados < 0) grados = grados * (-1); - if(minutos < 0) minutos = minutos * (-1); - - stringLongitudTemp = QString::number(grados)+ " ° "+ QString::number(minutos)+"' "+ tempEW; - - return stringLongitudTemp; - } - else - { - stringLongitudTemp = "erroneous longitude"; - return stringLongitudTemp; - } -} - -void WaypointGlobalView::changeOrbitalState(int state) -{ - Q_UNUSED(state); - - if(ui->m_orbitCheckBox->isChecked()) - { - ui->m_orbitalSpinBox->setEnabled(true); - ui->m_orbitalSpinBox->show(); - wp->setType(Waypoint::GLOBAL_ORBIT); - } - else - { - ui->m_orbitalSpinBox->setEnabled(false); - ui->m_orbitalSpinBox->hide(); - wp->setType(Waypoint::GLOBAL); - } -} - -void WaypointGlobalView::getLatitudeGradoMin(float latitud, int *gradoLat, float *minLat, QString *dirLat) -{ - - float minutos = 0; - float grados = 0; - float entero = 0; - float dec = 0; - - if (latitud<0){*dirLat="S"; latitud = latitud * -1;} - else {*dirLat="N";} - - if(latitud< 90 || latitud > -90) - { - dec = latitud - (entero = ::floor(latitud));; - minutos = dec * 60; - grados = entero; - if(grados < 0) grados = grados * (-1); - if(minutos < 0) minutos = minutos * (-1); - - *gradoLat = grados; - *minLat = minutos; - - - } - else - { - *gradoLat = -1; - *minLat = -1; - *dirLat="N/A"; - - } - -} - -void WaypointGlobalView::getLongitudGradoMin(float longitud, int *gradoLon, float *minLon, QString *dirLon) -{ - - float minutos = 0; - float grados = 0; - float entero = 0; - float dec = 0; - - if (longitud<0){*dirLon="W"; longitud = longitud * -1;} - else {*dirLon="E";} - - if(longitud<180 || longitud > -180) - { - dec = longitud - (entero = ::floor(longitud));; - minutos = dec * 60; - grados = entero; - if(grados < 0) grados = grados * (-1); - if(minutos < 0) minutos = minutos * (-1); - - *gradoLon = grados; - *minLon = minutos; - - } - else - { - *gradoLon = -1; - *minLon = -1; - *dirLon="N/A"; - } -} - -void WaypointGlobalView::updateCoordValues(float lat, float lon) -{ - -} - -void WaypointGlobalView::updateLatitudeWP(int value) -{ - Q_UNUSED(value); - - - int gradoLat; - float minLat; - float Latitud; - QString dirLat; - - gradoLat = ui->m_latitudGrados_spinBox->value(); - minLat = ui->m_latitudMinutos_spinBox->value(); - if(ui->m_dirLatitudeN_radioButton->isChecked()) - { - dirLat = "N"; - } - else - { - dirLat = "S"; - } - //dirLat = ui->m_dirLatitud_label->text(); - - Latitud = gradoLat + (minLat/60); - if(dirLat == "S"){Latitud = Latitud * -1;} - - wp->setY(Latitud); - - //emit signal waypoint position was changed - - emit changePositionWP(wp); - -} - -void WaypointGlobalView::updateLatitudeMinuteWP(double value) -{ - Q_UNUSED(value); - - int gradoLat; - float minLat; - float Latitud; - QString dirLat; - - gradoLat = ui->m_latitudGrados_spinBox->value(); - minLat = ui->m_latitudMinutos_spinBox->value(); - //dirLat = ui->m_dirLatitud_label->text(); - if(ui->m_dirLatitudeN_radioButton->isChecked()) - { - dirLat = "N"; - } - else - { - dirLat = "S"; - } - - Latitud = gradoLat + (minLat/60); - if(dirLat == "S"){Latitud = Latitud * -1;} - - wp->setY(Latitud); - - //emit signal waypoint position was changed - emit changePositionWP(wp); - - -} - -void WaypointGlobalView::updateLongitudeWP(int value) -{ - Q_UNUSED(value); - - int gradoLon; - float minLon; - float Longitud; - QString dirLon; - - gradoLon = ui->m_longitudGrados_spinBox->value(); - minLon = ui->m_longitudMinutos_spinBox->value(); - // dirLon = ui->m_dirLongitud_label->text(); - if(ui->m_dirLongitudeW_radioButton->isChecked()) - { - dirLon = "W"; - } - else - { - dirLon = "E"; - } - - Longitud = gradoLon + (minLon/60); - if(dirLon == "W"){Longitud = Longitud * -1;} - - wp->setX(Longitud); - - //emit signal waypoint position was changed - emit changePositionWP(wp); - - -} - - - -void WaypointGlobalView::updateLongitudeMinuteWP(double value) -{ - Q_UNUSED(value); - - int gradoLon; - float minLon; - float Longitud; - QString dirLon; - - gradoLon = ui->m_longitudGrados_spinBox->value(); - minLon = ui->m_longitudMinutos_spinBox->value(); - // dirLon = ui->m_dirLongitud_label->text(); - if(ui->m_dirLongitudeW_radioButton->isChecked()) - { - dirLon = "W"; - } - else - { - dirLon = "E"; - } - - Longitud = gradoLon + (minLon/60); - if(dirLon == "W"){Longitud = Longitud * -1;} - - wp->setX(Longitud); - - //emit signal waypoint position was changed - emit changePositionWP(wp); - -} - -void WaypointGlobalView::changeDirectionLatitudeWP() -{ - if(ui->m_dirLatitudeN_radioButton->isChecked()) - { - if(wp->getY() < 0) - { - wp->setY(wp->getY()* -1); - //emit signal waypoint position was changed - emit changePositionWP(wp); - } - - } - if(ui->m_dirLatitudeS_radioButton->isChecked()) - { - if(wp->getY() > 0) - { - wp->setY(wp->getY()*-1); - //emit signal waypoint position was changed - emit changePositionWP(wp); - - } - - } - -} - -void WaypointGlobalView::changeDirectionLongitudeWP() -{ - if(ui->m_dirLongitudeW_radioButton->isChecked()) - { - if(wp->getX() > 0) - { - wp->setX(wp->getX()*-1); - //emit signal waypoint position was changed - emit changePositionWP(wp); - } - - } - - if(ui->m_dirLongitudeE_radioButton->isChecked()) - { - if(wp->getX() < 0) - { - wp->setX(wp->getX()*-1); - //emit signal waypoint position was changed - emit changePositionWP(wp); - } - - } - - - -} - - diff --git a/src/ui/WaypointGlobalView.h b/src/ui/WaypointGlobalView.h deleted file mode 100644 index 02fc79591553078bf466e12dc266313dbddfc3f8..0000000000000000000000000000000000000000 --- a/src/ui/WaypointGlobalView.h +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef WAYPOINTGLOBALVIEW_H -#define WAYPOINTGLOBALVIEW_H - -#include -#include "Waypoint.h" - -namespace Ui { - class WaypointGlobalView; -} - -class WaypointGlobalView : public QWidget -{ - Q_OBJECT - -public: - explicit WaypointGlobalView(Waypoint* wp, QWidget *parent = 0); - ~WaypointGlobalView(); - -public slots: - - void updateValues(void); - void remove(); - QString getLatitudString(float lat); - QString getLongitudString(float lon); - void getLatitudeGradoMin(float lat, int *gradoLat, float *minLat, QString *dirLat); - void getLongitudGradoMin(float lon, int *gradoLon, float *minLon, QString *dirLon); - void changeOrbitalState(int state); - void updateCoordValues(float lat, float lon); - - - - //update latitude - void updateLatitudeWP(int value); - void updateLatitudeMinuteWP(double value); - void changeDirectionLatitudeWP(); - - //update longitude - void updateLongitudeWP(int value); - void updateLongitudeMinuteWP(double value); - void changeDirectionLongitudeWP(); - - - -signals: - - void removeWaypoint(Waypoint*); - void changePositionWP(Waypoint*); - - -protected: - virtual void changeEvent(QEvent *e); - - Waypoint* wp; - -private: - Ui::WaypointGlobalView *ui; - -private slots: - -}; - -#endif // WAYPOINTGLOBALVIEW_H diff --git a/src/ui/WaypointGlobalView.ui b/src/ui/WaypointGlobalView.ui deleted file mode 100644 index d3677b54fea1414430e93fef7b8b8be1c602d177..0000000000000000000000000000000000000000 --- a/src/ui/WaypointGlobalView.ui +++ /dev/null @@ -1,866 +0,0 @@ - - - WaypointGlobalView - - - - 0 - 0 - 824 - 79 - - - - - 0 - 0 - - - - - 50 - 0 - - - - Form - - - QWidget#colorIcon {} - -QWidget { -background-color: #252528; -color: #DDDDDF; -border-color: #EEEEEE; -background-clip: border; -} - -QCheckBox { -background-color: #252528; -color: #454545; -} - -QGroupBox { - border: 1px solid #EEEEEE; - border-radius: 5px; - padding: 0px 0px 0px 0px; -margin-top: 1ex; /* leave space at the top for the title */ - margin: 0px; -} - - QGroupBox::title { -     subcontrol-origin: margin; -     subcontrol-position: top center; /* position at the top center */ -     margin: 0 3px 0px 3px; -     padding: 0 3px 0px 0px; -     font: bold 8px; - } - -QGroupBox#heartbeatIcon { - background-color: red; -} - - QDockWidget { - font: bold; -    border: 1px solid #32345E; -} - -QPushButton { - font-weight: bold; - font-size: 12px; - border: 1px solid #999999; - border-radius: 10px; - min-width:22px; - max-width: 36px; - min-height: 16px; - max-height: 16px; - padding: 2px; - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #777777, stop: 1 #555555); -} - -QPushButton:pressed { - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #444444, stop: 1 #555555); -} - -QPushButton#landButton { - color: #000000; - background: qlineargradient(x1:0, y1:0, x2:0, y2:1, -                             stop:0 #ffee01, stop:1 #ae8f00) url("ICONDIR/control/emergency-button.png"); -} - -QPushButton:pressed#landButton { - color: #000000; - background: qlineargradient(x1:0, y1:0, x2:0, y2:1, -                             stop:0 #bbaa00, stop:1 #a05b00) url("ICONDIR/control/emergency-button.png"); -} - -QPushButton#killButton { - color: #000000; - background: qlineargradient(x1:0, y1:0, x2:0, y2:1, -                             stop:0 #ffb917, stop:1 #b37300) url("ICONDIR/control/emergency-button.png"); -} - -QPushButton:pressed#killButton { - color: #000000; - background: qlineargradient(x1:0, y1:0, x2:0, y2:1, -                             stop:0 #bb8500, stop:1 #903000) url("ICONDIR/control/emergency-button.png"); -} - -QProgressBar { - border: 1px solid white; - border-radius: 4px; - text-align: center; - padding: 2px; - color: white; - background-color: #111111; -} - -QProgressBar:horizontal { - height: 12px; -} - -QProgressBar QLabel { - font-size: 8px; -} - -QProgressBar:vertical { - width: 12px; -} - -QProgressBar::chunk { - background-color: #656565; -} - -QProgressBar::chunk#batteryBar { - background-color: green; -} - -QProgressBar::chunk#speedBar { - background-color: yellow; -} - -QProgressBar::chunk#thrustBar { - background-color: orange; -} - - - - 0 - - - 0 - - - - - - 0 - 0 - - - - - - - - 2 - - - 4 - - - 2 - - - 4 - - - - - - 10 - 75 - true - - - - WP_id - - - - - - - Qt::Horizontal - - - - 16 - 20 - - - - - - - - - 75 - true - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - true - - - - 2 - - - 2 - - - - - - 10 - 75 - true - - - - LAT: - - - - - - - ° - - - 0 - - - 89 - - - - - - - ' - - - 0.000000000000000 - - - 59.990000000000002 - - - 0.010000000000000 - - - - - - - S - - - - - - - N - - - true - - - - - - - - - - Qt::Horizontal - - - - 11 - 20 - - - - - - - - - 75 - true - - - - - - - - 2 - - - 2 - - - - - - 10 - 75 - true - - - - LON: - - - - - - - ° - - - -179 - - - 179 - - - - - - - ' - - - 0.000000000000000 - - - 59.990000000000002 - - - 0.010000000000000 - - - - - - - E - - - - - - - W - - - true - - - - - - - - - - Qt::Horizontal - - - - 16 - 20 - - - - - - - - - - - - 2 - - - 2 - - - - - Alt - - - - - - - - - - Orbital - - - - - - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 69 - 69 - 69 - - - - - - - 255 - 255 - 255 - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 37 - 37 - 40 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 69 - 69 - 69 - - - - - - - 255 - 255 - 255 - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 37 - 37 - 40 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 69 - 69 - 69 - - - - - - - 255 - 255 - 255 - - - - - - - 69 - 69 - 69 - - - - - - - 37 - 37 - 40 - - - - - - - 37 - 37 - 40 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - - - - - - - true - - - - - - - - - - - - - - diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 12b7e734cd99b2783f8667c46bc71a05fd40a85a..7c6f523b28100658878e247c07de1e14ca13b9b1 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -129,6 +129,7 @@ void WaypointList::setUAS(UASInterface* uas) connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void))); + connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); @@ -214,10 +215,10 @@ void WaypointList::add() wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); uas->getWaypointManager()->addWaypoint(wp); } - if (wp->getFrame() == MAV_FRAME_GLOBAL) - { - emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); - } +// if (wp->getFrame() == MAV_FRAME_GLOBAL) +// { +// emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); +// } } } } @@ -293,6 +294,13 @@ void WaypointList::currentWaypointChanged(quint16 seq) } } +void WaypointList::updateWaypoint(int uas, Waypoint* wp) +{ + Q_UNUSED(uas); + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); +} + void WaypointList::waypointListChanged() { if (uas) @@ -301,63 +309,6 @@ void WaypointList::waypointListChanged() this->setUpdatesEnabled(false); const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - // For Global Waypoints - //if(isGlobalWP) - //{ - //isLocalWP = false; - //// first remove all views of non existing waypoints - //if (!wpGlobalViews.empty()) - //{ - //QMapIterator viewIt(wpGlobalViews); - //viewIt.toFront(); - //while(viewIt.hasNext()) - //{ - //viewIt.next(); - //Waypoint *cur = viewIt.key(); - //int i; - //for (i = 0; i < waypoints.size(); i++) - //{ - //if (waypoints[i] == cur) - //{ - //break; - //} - //} - //if (i == waypoints.size()) - //{ - //WaypointGlobalView* widget = wpGlobalViews.find(cur).value(); - //widget->hide(); - //listLayout->removeWidget(widget); - //wpGlobalViews.remove(cur); - //} - //} - //} - - //// then add/update the views for each waypoint in the list - //for(int i = 0; i < waypoints.size(); i++) - //{ - //Waypoint *wp = waypoints[i]; - //if (!wpGlobalViews.contains(wp)) - //{ - //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this); - //wpGlobalViews.insert(wp, wpview); - //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*))); - //// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - //// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*))); - //// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - //// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); - //} - //WaypointGlobalView *wpgv = wpGlobalViews.value(wp); - //wpgv->updateValues(); - //listLayout->addWidget(wpgv); - //} - - //} - //else - //{ - // for local Waypoints - // first remove all views of non existing waypoints if (!wpViews.empty()) { QMapIterator viewIt(wpViews); @@ -408,7 +359,6 @@ void WaypointList::waypointListChanged() loadFileGlobalWP = false; - //} } void WaypointList::moveUp(Waypoint* wp) @@ -499,29 +449,6 @@ void WaypointList::on_clearWPListButton_clicked() } } -/** @brief Add a waypoint by mouse click over the map */ -void WaypointList::addWaypointMouse(QPointF coordinate) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager()->addWaypoint(wp); - } - else - { - Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000); - uas->getWaypointManager()->addWaypoint(wp); - } - - - } - -} - /** @brief The MapWidget informs that a waypoint global was changed on the map */ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index cb42b70db39eb79ff40c32d2edcd666153f8bf28..cb36e8bd5f2f589ae9e580d9b44bbc9708505bc9 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -41,7 +41,6 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include "UASInterface.h" #include "WaypointView.h" -#include "WaypointGlobalView.h" namespace Ui { @@ -65,7 +64,7 @@ public slots: /** @brief Save the local waypoint list to a file */ void saveWaypoints(); /** @brief Load a waypoint list from a file */ - void loadWaypoints(); + void loadWaypoints(); /** @brief Transmit the local waypoint list to the UAS */ void transmit(); /** @brief Read the remote waypoint list */ @@ -75,10 +74,6 @@ public slots: /** @brief Add a waypoint at the current MAV position */ void addCurrentPositonWaypoint(); /** @brief Add a waypoint by mouse click over the map */ - void addWaypointMouse(QPointF coordinate); - /** @brief it notifies that a global waypoint goes to do created */ - //void setIsWPGlobal(bool value, QPointF centerCoordinate); - //Update events /** @brief sets statusLabel string */ @@ -87,6 +82,8 @@ public slots: void changeCurrentWaypoint(quint16 seq); /** @brief The waypoint planner changed the current waypoint */ void currentWaypointChanged(quint16 seq); + /** @brief The waypoint manager informs that one waypoint was changed */ + void updateWaypoint(int uas, Waypoint* wp); /** @brief The waypoint manager informs that the waypoint list was changed */ void waypointListChanged(void); @@ -111,17 +108,12 @@ public slots: signals: void clearPathclicked(); void createWaypointAtMap(const QPointF coordinate); - // void ChangeWaypointGlobalPosition(int index, QPointF coord); - void changePositionWPBySpinBox(int indexWP, float lat, float lon); - - protected: virtual void changeEvent(QEvent *e); protected: QMap wpViews; - //QMap wpGlobalViews; QVBoxLayout* listLayout; UASInterface* uas; double mavX; diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index ca7f981a067b8a8ba2bd41551adce7edc30df990..13851500c1bdfd0f1818bb563a196f3dcd472031 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -1,24 +1,4 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009, 2010 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 . - +/*=================================================================== ======================================================================*/ /** @@ -43,8 +23,8 @@ This file is part of the PIXHAWK project #include "ui_WaypointView.h" WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : - QWidget(parent), - m_ui(new Ui::WaypointView) + QWidget(parent), + m_ui(new Ui::WaypointView) { m_ui->setupUi(this); @@ -146,7 +126,7 @@ void WaypointView::changedAction(int index) break; case MAV_ACTION_NAVIGATE: m_ui->autoContinue->show(); - m_ui->orbitSpinBox->show(); + m_ui->orbitSpinBox->show(); break; case MAV_ACTION_LOITER: m_ui->orbitSpinBox->show(); @@ -159,14 +139,6 @@ void WaypointView::changedAction(int index) void WaypointView::changedFrame(int index) { - // hide everything to start - m_ui->lonSpinBox->hide(); - m_ui->latSpinBox->hide(); - m_ui->altSpinBox->hide(); - m_ui->posNSpinBox->hide(); - m_ui->posESpinBox->hide(); - m_ui->posDSpinBox->hide(); - // set waypoint action MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt(); wp->setFrame(frame); @@ -174,11 +146,17 @@ void WaypointView::changedFrame(int index) switch(frame) { case MAV_FRAME_GLOBAL: + m_ui->posNSpinBox->hide(); + m_ui->posESpinBox->hide(); + m_ui->posDSpinBox->hide(); m_ui->lonSpinBox->show(); m_ui->latSpinBox->show(); m_ui->altSpinBox->show(); break; case MAV_FRAME_LOCAL: + m_ui->lonSpinBox->hide(); + m_ui->latSpinBox->hide(); + m_ui->altSpinBox->hide(); m_ui->posNSpinBox->show(); m_ui->posESpinBox->show(); m_ui->posDSpinBox->show(); @@ -192,34 +170,61 @@ void WaypointView::changedCurrent(int state) { if (state == 0) { - //m_ui->selectedBox->setChecked(true); - //m_ui->selectedBox->setCheckState(Qt::Checked); - //wp->setCurrent(false); + m_ui->selectedBox->setChecked(true); + m_ui->selectedBox->setCheckState(Qt::Checked); + wp->setCurrent(false); } else { - //wp->setCurrent(true); + wp->setCurrent(true); emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false } } void WaypointView::updateValues() { + // Deactivate signals from the WP + wp->blockSignals(true); // update frame MAV_FRAME frame = wp->getFrame(); int frame_index = m_ui->comboBox_frame->findData(frame); - m_ui->comboBox_frame->setCurrentIndex(frame_index); + if (m_ui->comboBox_frame->currentIndex() != frame_index) + { + m_ui->comboBox_frame->setCurrentIndex(frame_index); + } switch(frame) { case(MAV_FRAME_LOCAL): - m_ui->posNSpinBox->setValue(wp->getX()); - m_ui->posESpinBox->setValue(wp->getY()); - m_ui->posDSpinBox->setValue(wp->getZ()); + { + if (m_ui->posNSpinBox->value() != wp->getX()) + { + m_ui->posNSpinBox->setValue(wp->getX()); + } + if (m_ui->posESpinBox->value() != wp->getY()) + { + m_ui->posESpinBox->setValue(wp->getY()); + } + if (m_ui->posDSpinBox->value() != wp->getZ()) + { + m_ui->posDSpinBox->setValue(wp->getZ()); + } + } break; case(MAV_FRAME_GLOBAL): - m_ui->lonSpinBox->setValue(wp->getX()); - m_ui->latSpinBox->setValue(wp->getY()); - m_ui->altSpinBox->setValue(wp->getZ()); + { + if (m_ui->lonSpinBox->value() != wp->getX()) + { + m_ui->lonSpinBox->setValue(wp->getX()); + } + if (m_ui->latSpinBox->value() != wp->getY()) + { + m_ui->latSpinBox->setValue(wp->getY()); + } + if (m_ui->altSpinBox->value() != wp->getZ()) + { + m_ui->altSpinBox->setValue(wp->getZ()); + } + } break; } changedFrame(frame_index); @@ -227,7 +232,10 @@ void WaypointView::updateValues() // update action MAV_ACTION action = wp->getAction(); int action_index = m_ui->comboBox_action->findData(action); - m_ui->comboBox_action->setCurrentIndex(action_index); + if (m_ui->comboBox_action->currentIndex() != action_index) + { + m_ui->comboBox_action->setCurrentIndex(action_index); + } switch(action) { case MAV_ACTION_TAKEOFF: @@ -243,12 +251,28 @@ void WaypointView::updateValues() } changedAction(action_index); - m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); - m_ui->selectedBox->setChecked(wp->getCurrent()); - m_ui->autoContinue->setChecked(wp->getAutoContinue()); - m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\ - m_ui->orbitSpinBox->setValue(wp->getOrbit()); - m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); + if (m_ui->yawSpinBox->value() != wp->getYaw()/M_PI*180.) + { + m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); + } + if (m_ui->selectedBox->isChecked() != wp->getCurrent()) + { + m_ui->selectedBox->setChecked(wp->getCurrent()); + } + if (m_ui->autoContinue->isChecked() != wp->getAutoContinue()) + { + m_ui->autoContinue->setChecked(wp->getAutoContinue()); + } + m_ui->idLabel->setText(QString("%1").arg(wp->getId())); + if (m_ui->orbitSpinBox->value() != wp->getOrbit()) + { + m_ui->orbitSpinBox->setValue(wp->getOrbit()); + } + if (m_ui->holdTimeSpinBox->value() != wp->getHoldTime()) + { + m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); + } + wp->blockSignals(false); } void WaypointView::setCurrent(bool state) @@ -259,7 +283,7 @@ void WaypointView::setCurrent(bool state) } else { - m_ui->selectedBox->setCheckState(Qt::Unchecked); + m_ui->selectedBox->setCheckState(Qt::Unchecked); } } diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index 7e8610374231f8d08b192da014bf53ae912c2517..4615d6b48fd6e125a3ef6e668541d412f75a162c 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -170,6 +170,9 @@ QProgressBar::chunk#thrustBar { + + Qt::TabFocus + Currently selected waypoint @@ -231,9 +234,21 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + Position X coordinate + + false + + + true + + + true + N @@ -259,6 +274,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + Position Y coordinate @@ -290,6 +308,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + Position Z coordinate (negative) @@ -318,6 +339,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + lat @@ -333,6 +357,9 @@ QProgressBar::chunk#thrustBar { 90.000000000000000 + + 0.000010000000000 + @@ -343,6 +370,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + lon @@ -358,6 +388,9 @@ QProgressBar::chunk#thrustBar { 180.000000000000000 + + 0.000010000000000 + @@ -393,6 +426,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Yaw angle @@ -421,6 +457,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Orbit radius @@ -449,6 +488,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Time in milliseconds that the MAV has to stay inside the orbit before advancing @@ -477,6 +519,9 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Take off angle @@ -506,6 +551,9 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + Move Up @@ -526,6 +574,9 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + Move Down @@ -546,6 +597,9 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + Delete diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index 8920f76d996e626505b450d55620cd144e1f3630..ded3fe240d4682a6acbe2343ace61ec4e3fbaf98 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -1,23 +1,32 @@ #include "MAV2DIcon.h" #include -MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, QString name, Alignment alignment, QPen* pen) - : Point(x, y, name, alignment), - yaw(0.0f) +#include + +MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen) + : Point(uas->getLatitude(), uas->getLongitude(), name, alignment), + yaw(0.0f), + radius(radius), + type(type), + iconColor(color), + selected(uas->getSelected()), + uasid(uas->getUASID()) { + //connect size = QSize(radius, radius); drawIcon(pen); } MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen) - : Point(x, y, name, alignment) + : Point(x, y, name, alignment), + radius(20), + type(0), + iconColor(Qt::yellow), + selected(false), + uasid(0) { - int radius = 10; size = QSize(radius, radius); - if (pen) - { - drawIcon(pen); - } + drawIcon(pen); } MAV2DIcon::~MAV2DIcon() @@ -27,69 +36,179 @@ MAV2DIcon::~MAV2DIcon() void MAV2DIcon::setPen(QPen* pen) { - if (pen) - { - mypen = pen; - drawIcon(pen); - } + mypen = pen; + drawIcon(pen); +} + +void MAV2DIcon::setSelectedUAS(bool selected) +{ + this->selected = selected; + drawIcon(mypen); } /** + * Yaw changes of less than ±15 degrees are ignored. + * * @param yaw in radians, 0 = north, positive = clockwise */ void MAV2DIcon::setYaw(float yaw) { - this->yaw = yaw; - drawIcon(mypen); + //qDebug() << "MAV2Icon" << yaw; + float diff = fabs(yaw - this->yaw); + while (diff > M_PI) + { + diff -= M_PI; + } + + if (diff > 0.1) + { + this->yaw = yaw; + drawIcon(mypen); + } } void MAV2DIcon::drawIcon(QPen* pen) { - - mypixmap = new QPixmap(radius+1, radius+1); + Q_UNUSED(pen); + if (!mypixmap) mypixmap = new QPixmap(radius+1, radius+1); mypixmap->fill(Qt::transparent); QPainter painter(mypixmap); + painter.setRenderHint(QPainter::TextAntialiasing); + painter.setRenderHint(QPainter::Antialiasing); + painter.setRenderHint(QPainter::HighQualityAntialiasing); + + // Rotate by yaw + painter.translate(radius/2, radius/2); - // DRAW WAYPOINT - QPointF p(radius/2, radius/2); - - float waypointSize = radius; - QPolygonF poly(4); - // Top point - poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f)); - // Right point - poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y())); - // Bottom point - poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f)); - poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y())); - -// // Select color based on if this is the current waypoint -// if (list.at(i)->getCurrent()) -// { -// color = QGC::colorCyan;//uas->getColor(); -// pen.setWidthF(refLineWidthToPen(0.8f)); -// } -// else -// { -// color = uas->getColor(); -// pen.setWidthF(refLineWidthToPen(0.4f)); -// } - - //pen.setColor(color); - if (pen) + // Draw selected indicator + if (selected) { - pen->setWidthF(2); - painter.setPen(*pen); +// qDebug() << "SYSTEM IS NOW SELECTED"; +// QColor color(Qt::yellow); +// color.setAlpha(0.3f); + painter.setBrush(Qt::NoBrush); +// QPen selPen(color); +// int width = 5; +// selPen.setWidth(width); + QPen pen(Qt::yellow); + pen.setWidth(2); + painter.setPen(pen); + painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1); + //qDebug() << "Painting ellipse" << radius/2-width << width; + //selPen->deleteLater(); } - else + + switch (type) { - QPen pen2(Qt::red); - pen2.setWidth(2); - painter.setPen(pen2); - } - painter.setBrush(Qt::NoBrush); + case MAV_ICON_AIRPLANE: + { + // DRAW AIRPLANE - float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); - painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad); - painter.drawPolygon(poly); + // Rotate 180 degs more since the icon is oriented downwards + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + float iconSize = radius*0.9f; + QPolygonF poly(24); + poly.replace(0, QPointF(0.000000f*iconSize, -0.312500f*iconSize)); + poly.replace(1, QPointF(0.025000f*iconSize, -0.287500f*iconSize)); + poly.replace(2, QPointF(0.037500f*iconSize, -0.237500f*iconSize)); + poly.replace(3, QPointF(0.031250f*iconSize, -0.187500f*iconSize)); + poly.replace(4, QPointF(0.025000f*iconSize, -0.043750f*iconSize)); + poly.replace(5, QPointF(0.500000f*iconSize, -0.025000f*iconSize)); + poly.replace(6, QPointF(0.500000f*iconSize, 0.025000f*iconSize)); + poly.replace(7, QPointF(0.025000f*iconSize, 0.043750f*iconSize)); + poly.replace(8, QPointF(0.025000f*iconSize, 0.162500f*iconSize)); + poly.replace(9, QPointF(0.137500f*iconSize, 0.181250f*iconSize)); + poly.replace(10, QPointF(0.137500f*iconSize, 0.218750f*iconSize)); + poly.replace(11, QPointF(0.025000f*iconSize, 0.206250f*iconSize)); + poly.replace(12, QPointF(-0.025000f*iconSize, 0.206250f*iconSize)); + poly.replace(13, QPointF(-0.137500f*iconSize, 0.218750f*iconSize)); + poly.replace(14, QPointF(-0.137500f*iconSize, 0.181250f*iconSize)); + poly.replace(15, QPointF(-0.025000f*iconSize, 0.162500f*iconSize)); + poly.replace(16, QPointF(-0.025000f*iconSize, 0.043750f*iconSize)); + poly.replace(17, QPointF(-0.500000f*iconSize, 0.025000f*iconSize)); + poly.replace(18, QPointF(-0.500000f*iconSize, -0.025000f*iconSize)); + poly.replace(19, QPointF(-0.025000f*iconSize, -0.043750f*iconSize)); + poly.replace(20, QPointF(-0.031250f*iconSize, -0.187500f*iconSize)); + poly.replace(21, QPointF(-0.037500f*iconSize, -0.237500f*iconSize)); + poly.replace(22, QPointF(-0.031250f*iconSize, -0.262500f*iconSize)); + poly.replace(23, QPointF(0.000000f*iconSize, -0.312500f*iconSize)); + + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + } + break; + case MAV_ICON_QUADROTOR: + { + // QUADROTOR + float iconSize = radius*0.9f; + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + QPointF front(0, 0.2); + front = front *iconSize; + QPointF left(-0.2, 0); + left = left * iconSize; + QPointF right(0.2, 0.0); + right *= iconSize; + QPointF back(0, -0.2); + back *= iconSize; + + QPolygonF poly(0); + + + + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + + painter.drawEllipse(left, radius/4/2, radius/4/2); + painter.drawEllipse(right, radius/4/2, radius/4/2); + painter.drawEllipse(back, radius/4/2, radius/4/2); + + painter.setBrush(Qt::red); + painter.drawEllipse(front, radius/4/2, radius/4/2); + } + break; + case MAV_ICON_ROTARY_WING: + case MAV_ICON_GENERIC: + default: + { + // GENERIC + + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + float iconSize = radius*0.9f; + QPolygonF poly(3); + poly.replace(0, QPointF(0.0f*iconSize, 0.3f*iconSize)); + poly.replace(1, QPointF(-0.2f*iconSize, -0.2f*iconSize)); + poly.replace(2, QPointF(0.2f*iconSize, -0.2f*iconSize)); + + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + } + break; + } } diff --git a/src/ui/map/MAV2DIcon.h b/src/ui/map/MAV2DIcon.h index e59d196e5f1441e588ea9fc72664ea695ca25769..64173b9b4c16da2f6481de65d59d25600d657c98 100644 --- a/src/ui/map/MAV2DIcon.h +++ b/src/ui/map/MAV2DIcon.h @@ -1,32 +1,44 @@ #ifndef MAV2DICON_H #define MAV2DICON_H +#include #include "qmapcontrol.h" +#include "UASInterface.h" + class MAV2DIcon : public qmapcontrol::Point { public: + enum + { + MAV_ICON_GENERIC = 0, + MAV_ICON_AIRPLANE, + MAV_ICON_QUADROTOR, + MAV_ICON_ROTARY_WING + } MAV_ICON_TYPE; + + //! /*! * * @param x longitude * @param y latitude + * @param radius the radius of the circle * @param name name of the circle point * @param alignment alignment (Middle or TopLeft) * @param pen QPen for drawing */ - MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); + MAV2DIcon(UASInterface* uas, int radius = 10, int type=0, const QColor& color=QColor(Qt::red), QString name = QString(), Alignment alignment = Middle, QPen* pen=0); - //! /*! * * @param x longitude * @param y latitude - * @param radius the radius of the circle * @param name name of the circle point * @param alignment alignment (Middle or TopLeft) * @param pen QPen for drawing */ - MAV2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); + MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); + virtual ~MAV2DIcon(); //! sets the QPen which is used for drawing the circle @@ -37,13 +49,22 @@ public: */ virtual void setPen(QPen* pen); + /** @brief Mark this system as selected */ + void setSelectedUAS(bool selected); void setYaw(float yaw); + + /** @brief Get system id */ + int getUASId() const { return uasid; } + void drawIcon(QPen* pen); protected: - float yaw; ///< Yaw angle of the MAV - int radius; ///< Maximum dimension of the MAV icon - + float yaw; ///< Yaw angle of the MAV + int radius; ///< Radius / width of the icon + int type; ///< Type of aircraft: 0: generic, 1: airplane, 2: quadrotor, 3-n: rotary wing + QColor iconColor; ///< Color to be used for the icon + bool selected; ///< Wether this is the system currently in focus + int uasid; ///< ID of tracked system }; diff --git a/src/ui/map/Waypoint2DIcon.cc b/src/ui/map/Waypoint2DIcon.cc index 4afcee1c6057e59b3b86b4352728591fc38417b6..f7a45427a27a068eb9e1f3ab69e194dc0a99a118 100644 --- a/src/ui/map/Waypoint2DIcon.cc +++ b/src/ui/map/Waypoint2DIcon.cc @@ -6,6 +6,16 @@ Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, int radius, QString name, Align yaw(0.0f), radius(radius) { + waypoint = NULL; + size = QSize(radius, radius); + drawIcon(pen); +} + +Waypoint2DIcon::Waypoint2DIcon(Waypoint* wp, int listIndex, int radius, Alignment alignment, QPen* pen) + : Point(wp->getX(), wp->getY(), QString("%1").arg(listIndex), alignment) +{ + waypoint = wp; + this->yaw = wp->getYaw(); size = QSize(radius, radius); drawIcon(pen); } @@ -13,7 +23,8 @@ Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, int radius, QString name, Align Waypoint2DIcon::Waypoint2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* pen) : Point(x, y, name, alignment) { - int radius = 10; + waypoint = NULL; + int radius = 20; size = QSize(radius, radius); drawIcon(pen); } @@ -38,6 +49,18 @@ void Waypoint2DIcon::setYaw(float yaw) drawIcon(mypen); } +void Waypoint2DIcon::updateWaypoint() +{ + if (waypoint) + { + if (waypoint->getYaw() != yaw) + { + yaw = waypoint->getYaw(); + drawIcon(mypen); + } + } +} + void Waypoint2DIcon::drawIcon(QPen* pen) { mypixmap = new QPixmap(radius+1, radius+1); diff --git a/src/ui/map/Waypoint2DIcon.h b/src/ui/map/Waypoint2DIcon.h index 18c60973229ff1b1849bf4c2bf2ead57316f7ae1..d3bd5f570ae1b63a65e37692e3e54c92b0505374 100644 --- a/src/ui/map/Waypoint2DIcon.h +++ b/src/ui/map/Waypoint2DIcon.h @@ -4,6 +4,8 @@ #include #include "qmapcontrol.h" +#include "Waypoint.h" + class Waypoint2DIcon : public qmapcontrol::Point { public: @@ -27,7 +29,19 @@ public: * @param alignment alignment (Middle or TopLeft) * @param pen QPen for drawing */ - Waypoint2DIcon(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); + Waypoint2DIcon(qreal x, qreal y, int radius = 20, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); + + /** + * + * @param wp Waypoint + * @param listIndex Index in the waypoint list + * @param radius the radius of the circle + * @param name name of the circle point + * @param alignment alignment (Middle or TopLeft) + * @param pen QPen for drawing + */ + Waypoint2DIcon(Waypoint* wp, int listIndex, int radius = 20, Alignment alignment = Middle, QPen* pen=0); + virtual ~Waypoint2DIcon(); //! sets the QPen which is used for drawing the circle @@ -42,9 +56,13 @@ public: void drawIcon(QPen* pen); +public slots: + void updateWaypoint(); + protected: float yaw; ///< Yaw angle of the MAV - int radius; ///< + int radius; ///< Radius / diameter of the icon in pixels + Waypoint* waypoint; ///< Waypoint data container this icon represents }; diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 1ec7fe25453215a5db76d2f4ffce618fbe737734..52cbb30f0b825769eb144c9eeaba930756e57b27 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -185,6 +185,7 @@ void QGCGoogleEarthView::follow(bool follow) { ui->followAirplaneCheckbox->setChecked(follow); followCamera = follow; + if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow)); } void QGCGoogleEarthView::goHome() @@ -353,6 +354,8 @@ void QGCGoogleEarthView::initializeGoogleEarth() // Start update timer updateTimer->start(refreshRateMs); + follow(this->followCamera); + gEarthInitialized = true; } } @@ -395,11 +398,6 @@ void QGCGoogleEarthView::updateState() .arg(pitch, 0, 'f', 9) .arg(yaw, 0, 'f', 9)); } - - if (followCamera) - { - javaScript(QString("updateFollowAircraft()")); - } } } diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui index 62ed1354b22f3e691b0e054ead26dbf001918605..9fd726a4a071f113cdac3920bc17baffa3dd6acd 100644 --- a/src/ui/map3D/QGCGoogleEarthView.ui +++ b/src/ui/map3D/QGCGoogleEarthView.ui @@ -83,10 +83,10 @@ Distance of the chase camera to the MAV - 30 + 100 - 10000 + 20000 3000 diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 3a3f0d7d8ce5daf7aeff5cfabc92ab4061ab0f32..1861c92bd67eb5f8a082852ceda263e448ee8b0a 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -51,7 +51,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : uas(uas), load(0), state("UNKNOWN"), - stateDesc(tr("Unknown system state")), + stateDesc(tr("Unknown state")), mode("MAV_MODE_UNKNOWN"), thrust(0), isActive(false), @@ -66,6 +66,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : localFrame(false), removeAction(new QAction("Delete this system", this)), renameAction(new QAction("Rename..", this)), + selectAction(new QAction("Select this system", this )), m_ui(new Ui::UASView) { m_ui->setupUi(this); @@ -102,6 +103,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : // Allow to delete this widget connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater())); connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); + connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); // Name changes @@ -129,21 +131,15 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : m_ui->killButton->hide(); m_ui->shutdownButton->hide(); - if (localFrame) - { - m_ui->gpsLabel->hide(); - } - else - { - m_ui->positionLabel->hide(); - } - setSystemType(uas, uas->getSystemType()); } UASView::~UASView() { delete m_ui; + delete removeAction; + delete renameAction; + delete selectAction; } void UASView::heartbeatTimeout() @@ -332,8 +328,6 @@ void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double if (!localFrame) { localFrame = true; - m_ui->gpsLabel->hide(); - m_ui->positionLabel->show(); } } @@ -424,6 +418,7 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) { menu.addAction(removeAction); } + menu.addAction(selectAction); menu.exec(event->globalPos()); } @@ -471,7 +466,7 @@ void UASView::refresh() // Position QString position; - position = position.sprintf("%05.1f %05.1f %05.1f m", x, y, z); + position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z); m_ui->positionLabel->setText(position); QString globalPosition; QString latIndicator; @@ -492,17 +487,17 @@ void UASView::refresh() { lonIndicator = "W"; } - globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %05.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); - m_ui->gpsLabel->setText(globalPosition); + globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); + m_ui->positionLabel->setText(globalPosition); // Altitude if (groundDistance == 0 && alt != 0) { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 5, 'f', 1, '0')); + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0')); } else { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 5, 'f', 1, '0')); + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0')); } // Speed diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 1a62248c34dc101bb88c6c4b95b0004af12e5468..c2f76f386e8fddad102a223d66b0914cefe30396 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -110,6 +110,7 @@ protected: bool localFrame; QAction* removeAction; QAction* renameAction; + QAction* selectAction; static const int updateInterval = 300;