diff --git a/images/earth.html b/images/earth.html index 602799f14bdec5a7afe2db24ced99afcc85e5e87..825826a252b2f431d886f4126d0c6559c8d6907b 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) @@ -137,7 +142,8 @@ function createAircraft(id, type, color) aircraft[id] = planePlacemark; attitudes[id] = planeOrient; aircraftLocations[id] = planeLoc; - aircraftLastLocations[id] = ge.createLocation(''); + aircraftLastLocations[id] = ge.createLocation(''); + aircraftColors[id] = color; //planeColor = color; createTrail(id, color); @@ -172,7 +178,7 @@ trailsVisible[id] = false; } -function hideTrail(id) +function clearTrail(id) { trailsVisible[id] = false; ge.getFeatures().removeChild(trailPlacemarks[id]); @@ -184,6 +190,12 @@ function showTrail(id) trailsVisible[id] = true; } +function startTrail(id) +{ + createTrail(id, trailColors[id]); + trailsVisible[id] = true; +} + function setViewRange(dist) { currViewRange = dist; @@ -214,8 +226,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 +245,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+180.0); + + currFollowHeading = ((yaw/M_PI))*180.0+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 +304,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/lib/nmea/CMakeLists.txt b/lib/nmea/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..e7c658e5afd7dbe7c0ef67765c02ce0fe438580e --- /dev/null +++ b/lib/nmea/CMakeLists.txt @@ -0,0 +1,17 @@ +INCLUDE_DIRECTORIES(include) + +SET_SOURCE_FILES(NMEALIB_SRC_FILES + context.c + time.c + generate.c + generator.c + gmath.c + info.c + parse.c + parser.c + sentence.c + tok.c +) + +PIXHAWK_LIBRARY(nmea SHARED ${NMEALIB_SRC_FILES}) + diff --git a/lib/nmea/LICENSE.TXT b/lib/nmea/LICENSE.TXT new file mode 100755 index 0000000000000000000000000000000000000000..807db791666c16ff2c018a2146d1b69e7dbcb491 --- /dev/null +++ b/lib/nmea/LICENSE.TXT @@ -0,0 +1,506 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + + + diff --git a/lib/nmea/README.TXT b/lib/nmea/README.TXT new file mode 100755 index 0000000000000000000000000000000000000000..cc79c13d0dbd318e064a4c466ddfdaa51147c880 --- /dev/null +++ b/lib/nmea/README.TXT @@ -0,0 +1,26 @@ +NMEA library + +Disclaimer + +The National Marine Electronics Association (NMEA) has developed a specification that defines the interface between various pieces of marine electronic equipment. The standard permits marine electronics to send information to computers and to other marine equipment. +Most computer programs that provide real time position information understand and expect data to be in NMEA format. This data includes the complete PVT (position, velocity, time) solution computed by the GPS receiver. The idea of NMEA is to send a line of data called a sentence that is totally self contained and independent from other sentences. All NMEA sentences is sequences of ACSII symbols begins with a '$' and ends with a carriage return/line feed sequence and can be no longer than 80 characters of visible text (plus the line terminators). + +Introduction + +We present library in 'C' programming language for work with NMEA protocol. Small and easy to use. The library build on different compilers under different platforms (see below). The code was tested in real projects. Just download and try... + +Features + +- Analysis NMEA sentences and granting GPS data in C structures +- Generate NMEA sentences +- Supported sentences: GPGGA, GPGSA, GPGSV, GPRMC, GPVTG +- Multilevel architecture of algorithms +- Additional functions of geographical mathematics and work with navigation data + +Supported (tested) platforms + +- Microsoft Windows (MS Visual Studio 8.0, GCC) +- Windows Mobile, Windows CE (MS Visual Studio 8.0) +- UNIX (GCC) + +Licence: LGPL diff --git a/lib/nmea/context.c b/lib/nmea/context.c new file mode 100755 index 0000000000000000000000000000000000000000..89f04311e89fd8fd4c894b37ebcb0ebcef22749e --- /dev/null +++ b/lib/nmea/context.c @@ -0,0 +1,67 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/context.h" + +#include +#include +#include + +nmeaPROPERTY * nmea_property() +{ + static nmeaPROPERTY prop = { + 0, 0, NMEA_DEF_PARSEBUFF + }; + + return ∝ +} + +void nmea_trace(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaTraceFunc func = nmea_property()->trace_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} + +void nmea_trace_buff(const char *buff, int buff_size) +{ + nmeaTraceFunc func = nmea_property()->trace_func; + if(func && buff_size) + (*func)(buff, buff_size); +} + +void nmea_error(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaErrorFunc func = nmea_property()->error_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} diff --git a/lib/nmea/generate.c b/lib/nmea/generate.c new file mode 100755 index 0000000000000000000000000000000000000000..f7779bf5a5f975442b218b68f01d294a34e7e550 --- /dev/null +++ b/lib/nmea/generate.c @@ -0,0 +1,229 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/tok.h" +#include "nmea/sentence.h" +#include "nmea/generate.h" +#include "nmea/units.h" + +#include +#include +#include + +int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGGA,%02d%02d%02d.%02d,%07.4f,%C,%07.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->lat, pack->ns, pack->lon, pack->ew, + pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units, + pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid); +} + +int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f", + pack->fix_mode, pack->fix_type, + pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5], + pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11], + pack->PDOP, pack->HDOP, pack->VDOP); +} + +int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSV,%1d,%1d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d", + pack->pack_count, pack->pack_index + 1, pack->sat_count, + pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig, + pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig, + pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig, + pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig); +} + +int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPRMC,%02d%02d%02d.%02d,%C,%07.4f,%C,%07.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->status, pack->lat, pack->ns, pack->lon, pack->ew, + pack->speed, pack->direction, + pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100, + pack->declination, pack->declin_ew, pack->mode); +} + +int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C", + pack->dir, pack->dir_t, + pack->dec, pack->dec_m, + pack->spn, pack->spn_n, + pack->spk, pack->spk_k); +} + +void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack) +{ + nmea_zero_GPGGA(pack); + + pack->utc = info->utc; + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->sig = info->sig; + pack->satinuse = info->satinfo.inuse; + pack->HDOP = info->HDOP; + pack->elv = info->elv; +} + +void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack) +{ + int it; + + nmea_zero_GPGSA(pack); + + pack->fix_type = info->fix; + pack->PDOP = info->PDOP; + pack->HDOP = info->HDOP; + pack->VDOP = info->VDOP; + + for(it = 0; it < NMEA_MAXSAT; ++it) + { + pack->sat_prn[it] = + ((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0); + } +} + +int nmea_gsv_npack(int sat_count) +{ + int pack_count = (int)ceil(((double)sat_count) / NMEA_SATINPACK); + + if(0 == pack_count) + pack_count = 1; + + return pack_count; +} + +void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx) +{ + int sit, pit; + + nmea_zero_GPGSV(pack); + + pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT; + pack->pack_count = nmea_gsv_npack(pack->sat_count); + + if(pack->pack_count == 0) + pack->pack_count = 1; + + if(pack_idx >= pack->pack_count) + pack->pack_index = pack_idx % pack->pack_count; + else + pack->pack_index = pack_idx; + + for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit) + pack->sat_data[pit] = info->satinfo.sat[sit]; +} + +void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) +{ + nmea_zero_GPRMC(pack); + + pack->utc = info->utc; + pack->status = ((info->sig > 0)?'A':'V'); + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->speed = info->speed / NMEA_TUD_KNOTS; + pack->direction = info->direction; + pack->declination = info->declination; + pack->declin_ew = 'E'; + pack->mode = ((info->sig > 0)?'A':'N'); +} + +void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) +{ + nmea_zero_GPVTG(pack); + + pack->dir = info->direction; + pack->dec = info->declination; + pack->spn = info->speed / NMEA_TUD_KNOTS; + pack->spk = info->speed; +} + +int nmea_generate( + char *buff, int buff_sz, + const nmeaINFO *info, + int generate_mask + ) +{ + int gen_count = 0, gsv_it, gsv_count; + int pack_mask = generate_mask; + + nmeaGPGGA gga; + nmeaGPGSA gsa; + nmeaGPGSV gsv; + nmeaGPRMC rmc; + nmeaGPVTG vtg; + + if(!buff) + return 0; + + while(pack_mask) + { + if(pack_mask & GPGGA) + { + nmea_info2GPGGA(info, &gga); + gen_count += nmea_gen_GPGGA(buff + gen_count, buff_sz - gen_count, &gga); + pack_mask &= ~GPGGA; + } + else if(pack_mask & GPGSA) + { + nmea_info2GPGSA(info, &gsa); + gen_count += nmea_gen_GPGSA(buff + gen_count, buff_sz - gen_count, &gsa); + pack_mask &= ~GPGSA; + } + else if(pack_mask & GPGSV) + { + gsv_count = nmea_gsv_npack(info->satinfo.inview); + for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it) + { + nmea_info2GPGSV(info, &gsv, gsv_it); + gen_count += nmea_gen_GPGSV(buff + gen_count, buff_sz - gen_count, &gsv); + } + pack_mask &= ~GPGSV; + } + else if(pack_mask & GPRMC) + { + nmea_info2GPRMC(info, &rmc); + gen_count += nmea_gen_GPRMC(buff + gen_count, buff_sz - gen_count, &rmc); + pack_mask &= ~GPRMC; + } + else if(pack_mask & GPVTG) + { + nmea_info2GPVTG(info, &vtg); + gen_count += nmea_gen_GPVTG(buff + gen_count, buff_sz - gen_count, &vtg); + pack_mask &= ~GPVTG; + } + else + break; + + if(buff_sz - gen_count <= 0) + break; + } + + return gen_count; +} diff --git a/lib/nmea/generator.c b/lib/nmea/generator.c new file mode 100755 index 0000000000000000000000000000000000000000..7dd0618d9864fa7e43da278f668415ea898bccef --- /dev/null +++ b/lib/nmea/generator.c @@ -0,0 +1,404 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/gmath.h" +#include "nmea/generate.h" +#include "nmea/generator.h" +#include "nmea/context.h" + +#include +#include + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(disable: 4100) /* unreferenced formal parameter */ +#endif + +double nmea_random(double min, double max) +{ + static double rand_max = RAND_MAX; + double rand_val = rand(); + double bounds = max - min; + return min + (rand_val * bounds) / rand_max; +} + +/* + * low level + */ + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; int smask = info->smask; + nmeaGENERATOR *igen = gen; + + nmea_zero_INFO(info); + info->smask = smask; + + info->lat = NMEA_DEF_LAT; + info->lon = NMEA_DEF_LON; + + while(RetVal && igen) + { + if(igen->init_call) + RetVal = (*igen->init_call)(igen, info); + igen = igen->next; + } + + return RetVal; +} + +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->loop_call) + RetVal = (*gen->loop_call)(gen, info); + + if(RetVal && gen->next) + RetVal = nmea_gen_loop(gen->next, info); + + return RetVal; +} + +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->reset_call) + RetVal = (*gen->reset_call)(gen, info); + + return RetVal; +} + +void nmea_gen_destroy(nmeaGENERATOR *gen) +{ + if(gen->next) + { + nmea_gen_destroy(gen->next); + gen->next = 0; + } + + if(gen->destroy_call) + (*gen->destroy_call)(gen); + + free(gen); +} + +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen) +{ + if(to->next) + nmea_gen_add(to->next, gen); + else + to->next = gen; +} + +int nmea_generate_from( + char *buff, int buff_sz, + nmeaINFO *info, + nmeaGENERATOR *gen, + int generate_mask + ) +{ + int retval; + + if(0 != (retval = nmea_gen_loop(gen, info))) + retval = nmea_generate(buff, buff_sz, info, generate_mask); + + return retval; +} + +/* + * NOISE generator + */ + +int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + Q_UNUSED(gen); + return 1; +} + +int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int in_use; + + info->sig = (int)nmea_random(1, 3); + info->PDOP = nmea_random(0, 9); + info->HDOP = nmea_random(0, 9); + info->VDOP = nmea_random(0, 9); + info->fix = (int)nmea_random(2, 3); + info->lat = nmea_random(0, 100); + info->lon = nmea_random(0, 100); + info->speed = nmea_random(0, 100); + info->direction = nmea_random(0, 360); + info->declination = nmea_random(0, 360); + info->elv = (int)nmea_random(-100, 100); + + info->satinfo.inuse = 0; + info->satinfo.inview = 0; + + for(it = 0; it < 12; ++it) + { + info->satinfo.sat[it].id = it; + info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); + info->satinfo.sat[it].elv = (int)nmea_random(0, 90); + info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359); + info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); + + if(in_use) + info->satinfo.inuse++; + if(info->satinfo.sat[it].sig > 0) + info->satinfo.inview++; + } + + return 1; +} + +int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + Q_UNUSED(gen); + return 1; +} + +/* + * STATIC generator + */ + +int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + Q_UNUSED(gen); + nmea_time_now(&info->utc); + return 1; +}; + +int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + Q_UNUSED(gen); + info->satinfo.inuse = 4; + info->satinfo.inview = 4; + + info->satinfo.sat[0].id = 1; + info->satinfo.sat[0].in_use = 1; + info->satinfo.sat[0].elv = 50; + info->satinfo.sat[0].azimuth = 0; + info->satinfo.sat[0].sig = 99; + + info->satinfo.sat[1].id = 2; + info->satinfo.sat[1].in_use = 1; + info->satinfo.sat[1].elv = 50; + info->satinfo.sat[1].azimuth = 90; + info->satinfo.sat[1].sig = 99; + + info->satinfo.sat[2].id = 3; + info->satinfo.sat[2].in_use = 1; + info->satinfo.sat[2].elv = 50; + info->satinfo.sat[2].azimuth = 180; + info->satinfo.sat[2].sig = 99; + + info->satinfo.sat[3].id = 4; + info->satinfo.sat[3].in_use = 1; + info->satinfo.sat[3].elv = 50; + info->satinfo.sat[3].azimuth = 270; + info->satinfo.sat[3].sig = 99; + + return 1; +} + +int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_static_reset(gen, info); + + return 1; +} + +/* + * SAT_ROTATE generator + */ + +int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int count = info->satinfo.inview; + double deg = 360 / (count?count:1); + double srt = (count?(info->satinfo.sat[0].azimuth):0) + 5; + + nmea_time_now(&info->utc); + + for(it = 0; it < count; ++it) + { + info->satinfo.sat[it].azimuth = + (int)((srt >= 360)?srt - 360:srt); + srt += deg; + } + + return 1; +}; + +int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + double deg = 360 / 8; + double srt = 0; + + info->satinfo.inuse = 8; + info->satinfo.inview = 8; + + for(it = 0; it < info->satinfo.inview; ++it) + { + info->satinfo.sat[it].id = it + 1; + info->satinfo.sat[it].in_use = 1; + info->satinfo.sat[it].elv = 5; + info->satinfo.sat[it].azimuth = (int)srt; + info->satinfo.sat[it].sig = 80; + srt += deg; + } + + return 1; +} + +int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_rotate_reset(gen, info); + + return 1; +} + +/* + * POS_RANDMOVE generator + */ + +int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + info->direction = info->declination = 0; + info->speed = 20; + return 1; +} + +int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + Q_UNUSED(gen); + nmeaPOS crd; + + info->direction += nmea_random(-10, 10); + info->speed += nmea_random(-2, 3); + + if(info->direction < 0) + info->direction = 359 + info->direction; + if(info->direction > 359) + info->direction -= 359; + + if(info->speed > 40) + info->speed = 40; + if(info->speed < 1) + info->speed = 1; + + nmea_info2pos(info, &crd); + nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600); + nmea_pos2info(&crd, info); + + info->declination = info->direction; + + return 1; +}; + +int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen) +{ + return 1; +}; + +/* + * generator create + */ + +nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = 0; + + switch(type) + { + case NMEA_GEN_NOISE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_noise_init; + gen->loop_call = &nmea_igen_noise_loop; + gen->reset_call = &nmea_igen_noise_reset; + } + break; + case NMEA_GEN_STATIC: + case NMEA_GEN_SAT_STATIC: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_static_init; + gen->loop_call = &nmea_igen_static_loop; + gen->reset_call = &nmea_igen_static_reset; + } + break; + case NMEA_GEN_SAT_ROTATE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_rotate_init; + gen->loop_call = &nmea_igen_rotate_loop; + gen->reset_call = &nmea_igen_rotate_reset; + } + break; + case NMEA_GEN_POS_RANDMOVE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_pos_rmove_init; + gen->loop_call = &nmea_igen_pos_rmove_loop; + gen->destroy_call = &nmea_igen_pos_rmove_destroy; + } + break; + case NMEA_GEN_ROTATE: + gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); + nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); + break; + }; + + return gen; +} + +nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = __nmea_create_generator(type, info); + + if(gen) + nmea_gen_init(gen, info); + + return gen; +} + +void nmea_destroy_generator(nmeaGENERATOR *gen) +{ + nmea_gen_destroy(gen); +} + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(default: 4100) +#endif diff --git a/lib/nmea/gmath.c b/lib/nmea/gmath.c new file mode 100755 index 0000000000000000000000000000000000000000..766daefd81ae92254ed23f103cc26c4f7e2587c0 --- /dev/null +++ b/lib/nmea/gmath.c @@ -0,0 +1,377 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file gmath.h */ + +#include "nmea/gmath.h" + +#include +#include + +/** + * \fn nmea_degree2radian + * \brief Convert degree to radian + */ +double nmea_degree2radian(double val) +{ return (val * NMEA_PI180); } + +/** + * \fn nmea_radian2degree + * \brief Convert radian to degree + */ +double nmea_radian2degree(double val) +{ return (val / NMEA_PI180); } + +/** + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +double nmea_ndeg2degree(double val) +{ + double deg = ((int)(val / 100)); + val = deg + (val - deg * 100) / 60; + return val; +} + +/** + * \brief Convert fractional degree to NDEG (NMEA degree) + */ +double nmea_degree2ndeg(double val) +{ + double int_part; + double fra_part; + fra_part = modf(val, &int_part); + val = int_part * 100 + fra_part * 60; + return val; +} + +/** + * \fn nmea_ndeg2radian + * \brief Convert NDEG (NMEA degree) to radian + */ +double nmea_ndeg2radian(double val) +{ return nmea_degree2radian(nmea_ndeg2degree(val)); } + +/** + * \fn nmea_radian2ndeg + * \brief Convert radian to NDEG (NMEA degree) + */ +double nmea_radian2ndeg(double val) +{ return nmea_degree2ndeg(nmea_radian2degree(val)); } + +/** + * \brief Calculate PDOP (Position Dilution Of Precision) factor + */ +double nmea_calc_pdop(double hdop, double vdop) +{ + return sqrt(pow(hdop, 2) + pow(vdop, 2)); +} + +double nmea_dop2meters(double dop) +{ return (dop * NMEA_DOP_FACTOR); } + +double nmea_meters2dop(double meters) +{ return (meters / NMEA_DOP_FACTOR); } + +/** + * \brief Calculate distance between two points + * \return Distance in meters + */ +double nmea_distance( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos /**< To position in radians */ + ) +{ + double dist = ((double)NMEA_EARTHRADIUS_M) * acos( + sin(to_pos->lat) * sin(from_pos->lat) + + cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) + ); + return dist; +} + +/** + * \brief Calculate distance between two points + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + * \return Distance in meters + */ +double nmea_distance_ellipsoid( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos, /**< To position in radians */ + double *from_azimuth, /**< (O) azimuth at "from" position in radians */ + double *to_azimuth /**< (O) azimuth at "to" position in radians */ + ) +{ + /* All variables */ + double f, a, b, sqr_a, sqr_b; + double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; + double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; + int remaining_steps; + double sqr_u, A, B, delta_sigma; + + /* Check input */ + NMEA_ASSERT(from_pos != 0); + NMEA_ASSERT(to_pos != 0); + + if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) + { /* Identical points */ + if ( from_azimuth != 0 ) + *from_azimuth = 0; + if ( to_azimuth != 0 ) + *to_azimuth = 0; + return 0; + } /* Identical points */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + L = to_pos->lon - from_pos->lon; + phi1 = from_pos->lat; + phi2 = to_pos->lat; + U1 = atan((1 - f) * tan(phi1)); + U2 = atan((1 - f) * tan(phi2)); + sin_U1 = sin(U1); + sin_U2 = sin(U2); + cos_U1 = cos(U1); + cos_U2 = cos(U2); + + /* Initialize iteration */ + sigma = 0; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = 0; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sqr_cos_alpha = 0; + lambda = L; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + delta_lambda = lambda; + remaining_steps = 20; + + while ((delta_lambda > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + /* Variables */ + double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; + + /* Calculation */ + tmp1 = cos_U2 * sin_lambda; + tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; + sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); + cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; + tan_sigma = sin_sigma / cos_sigma; + sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; + cos_alpha = cos(asin(sin_alpha)); + sqr_cos_alpha = cos_alpha * cos_alpha; + cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + lambda_prev = lambda; + sigma = asin(sin_sigma); + lambda = L + + (1 - C) * f * sin_alpha + * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); + delta_lambda = lambda_prev - lambda; + if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + remaining_steps--; + } /* Iterate */ + + /* More calculation */ + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + + /* Calculate result */ + if ( from_azimuth != 0 ) + { + double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); + *from_azimuth = atan(tan_alpha_1); + } + if ( to_azimuth != 0 ) + { + double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); + *to_azimuth = atan(tan_alpha_2); + } + + return b * A * (sigma - delta_sigma); +} + +/** + * \brief Horizontal move of point position + */ +int nmea_move_horz( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< Result position in radians */ + double azimuth, /**< Azimuth (degree) [0, 359] */ + double distance /**< Distance (km) */ + ) +{ + nmeaPOS p1 = *start_pos; + int RetVal = 1; + + distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ + azimuth = nmea_degree2radian(azimuth); + + end_pos->lat = asin( + sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); + end_pos->lon = p1.lon + atan2( + sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); + + if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) + { + end_pos->lat = 0; end_pos->lon = 0; + RetVal = 0; + } + + return RetVal; +} + +/** + * \brief Horizontal move of point position + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + */ +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< (O) Result position in radians */ + double azimuth, /**< Azimuth in radians */ + double distance, /**< Distance (km) */ + double *end_azimuth /**< (O) Azimuth at end position in radians */ + ) +{ + /* Variables */ + double f, a, b, sqr_a, sqr_b; + double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; + double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; + double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; + int remaining_steps; + double tmp1, phi2, lambda, C, L; + + /* Check input */ + NMEA_ASSERT(start_pos != 0); + NMEA_ASSERT(end_pos != 0); + + if (fabs(distance) < 1e-12) + { /* No move */ + *end_pos = *start_pos; + if ( end_azimuth != 0 ) *end_azimuth = azimuth; + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); + } /* No move */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + phi1 = start_pos->lat; + tan_U1 = (1 - f) * tan(phi1); + cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); + sin_U1 = tan_U1 * cos_U1; + s = distance; + alpha1 = azimuth; + sin_alpha1 = sin(alpha1); + cos_alpha1 = cos(alpha1); + tan_sigma1 = tan_U1 / cos_alpha1; + sigma1 = atan2(tan_U1, cos_alpha1); + sin_alpha = cos_U1 * sin_alpha1; + sqr_cos_alpha = 1 - sin_alpha * sin_alpha; + cos_alpha = sqrt(sqr_cos_alpha); + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + + /* Initialize iteration */ + sigma_initial = s / (b * A); + sigma = sigma_initial; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + delta_sigma = 0; + sigma_prev = 2 * NMEA_PI; + remaining_steps = 20; + + while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + sigma_prev = sigma; + sigma = sigma_initial + delta_sigma; + remaining_steps --; + } /* Iterate */ + + /* Calculate result */ + tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); + phi2 = atan2( + sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, + (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) + ); + lambda = atan2( + sin_sigma * sin_alpha1, + cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 + ); + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + L = lambda - + (1 - C) * f * sin_alpha * ( + sigma + C * sin_sigma * + (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) + ); + + /* Result */ + end_pos->lon = start_pos->lon + L; + end_pos->lat = phi2; + if ( end_azimuth != 0 ) + { + *end_azimuth = atan2( + sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 + ); + } + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); +} + +/** + * \brief Convert position from INFO to radians position + */ +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos) +{ + pos->lat = nmea_ndeg2radian(info->lat); + pos->lon = nmea_ndeg2radian(info->lon); +} + +/** + * \brief Convert radians position to INFOs position + */ +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info) +{ + info->lat = nmea_radian2ndeg(pos->lat); + info->lon = nmea_radian2ndeg(pos->lon); +} diff --git a/lib/nmea/include/nmea/config.h b/lib/nmea/include/nmea/config.h new file mode 100755 index 0000000000000000000000000000000000000000..5014662187923220b410287f19e1ce79e917d495 --- /dev/null +++ b/lib/nmea/include/nmea/config.h @@ -0,0 +1,51 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_CONFIG_H__ +#define __NMEA_CONFIG_H__ + +#define NMEA_VERSION ("0.5.3") +#define NMEA_VERSION_MAJOR (0) +#define NMEA_VERSION_MINOR (5) +#define NMEA_VERSION_PATCH (3) + +#define NMEA_CONVSTR_BUF (256) +#define NMEA_TIMEPARSE_BUF (256) + +#if defined(WINCE) || defined(UNDER_CE) +# define NMEA_CE +#endif + +#if defined(WIN32) || defined(NMEA_CE) +# define NMEA_WIN +#else +# define NMEA_UNI +#endif + +#if defined(NMEA_WIN) && (_MSC_VER >= 1400) +# pragma warning(disable: 4996) /* declared deprecated */ +#endif + +#if defined(_MSC_VER) +# define NMEA_POSIX(x) _##x +# define NMEA_INLINE __inline +#else +# define NMEA_POSIX(x) x +# define NMEA_INLINE inline +#endif + +#if !defined(NDEBUG) && !defined(NMEA_CE) +# include +# define NMEA_ASSERT(x) assert(x) +#else +# define NMEA_ASSERT(x) +#endif + +#endif /* __NMEA_CONFIG_H__ */ diff --git a/lib/nmea/include/nmea/context.h b/lib/nmea/include/nmea/context.h new file mode 100755 index 0000000000000000000000000000000000000000..24600ad6ee4e16041b3a7cec4da845fce78dbc85 --- /dev/null +++ b/lib/nmea/include/nmea/context.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_CONTEXT_H__ +#define __NMEA_CONTEXT_H__ + +#include "config.h" + +#define NMEA_DEF_PARSEBUFF (1024) +#define NMEA_MIN_PARSEBUFF (256) + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void (*nmeaTraceFunc)(const char *str, int str_size); +typedef void (*nmeaErrorFunc)(const char *str, int str_size); + +typedef struct _nmeaPROPERTY +{ + nmeaTraceFunc trace_func; + nmeaErrorFunc error_func; + int parse_buff_size; + +} nmeaPROPERTY; + +nmeaPROPERTY * nmea_property(); + +void nmea_trace(const char *str, ...); +void nmea_trace_buff(const char *buff, int buff_size); +void nmea_error(const char *str, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_CONTEXT_H__ */ diff --git a/lib/nmea/include/nmea/generate.h b/lib/nmea/include/nmea/generate.h new file mode 100755 index 0000000000000000000000000000000000000000..9d7fdee51d54ac61e6bc63910f307d78bc1c652c --- /dev/null +++ b/lib/nmea/include/nmea/generate.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATE_H__ +#define __NMEA_GENERATE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_generate( + char *buff, int buff_sz, /* buffer */ + const nmeaINFO *info, /* source info */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack); +int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack); +int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack); +int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack); +int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack); + +void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack); +void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack); +void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack); +void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack); + +int nmea_gsv_npack(int sat_count); +void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATE_H__ */ diff --git a/lib/nmea/include/nmea/generator.h b/lib/nmea/include/nmea/generator.h new file mode 100755 index 0000000000000000000000000000000000000000..a97b91b1373da18bc708c245fcf02c2f171ce1be --- /dev/null +++ b/lib/nmea/include/nmea/generator.h @@ -0,0 +1,79 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATOR_H__ +#define __NMEA_GENERATOR_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +struct _nmeaGENERATOR; + +enum nmeaGENTYPE +{ + NMEA_GEN_NOISE = 0, + NMEA_GEN_STATIC, + NMEA_GEN_ROTATE, + + NMEA_GEN_SAT_STATIC, + NMEA_GEN_SAT_ROTATE, + NMEA_GEN_POS_RANDMOVE, + + NMEA_GEN_LAST +}; + +struct _nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info); +void nmea_destroy_generator(struct _nmeaGENERATOR *gen); + +int nmea_generate_from( + char *buff, int buff_sz, /* buffer */ + nmeaINFO *info, /* source info */ + struct _nmeaGENERATOR *gen, /* generator */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +/* + * low level + */ + +typedef int (*nmeaNMEA_GEN_INIT)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_LOOP)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_RESET)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_DESTROY)(struct _nmeaGENERATOR *gen); + +typedef struct _nmeaGENERATOR +{ + void *gen_data; + nmeaNMEA_GEN_INIT init_call; + nmeaNMEA_GEN_LOOP loop_call; + nmeaNMEA_GEN_RESET reset_call; + nmeaNMEA_GEN_DESTROY destroy_call; + struct _nmeaGENERATOR *next; + +} nmeaGENERATOR; + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info); +void nmea_gen_destroy(nmeaGENERATOR *gen); +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATOR_H__ */ diff --git a/lib/nmea/include/nmea/gmath.h b/lib/nmea/include/nmea/gmath.h new file mode 100755 index 0000000000000000000000000000000000000000..063dc632302d978a87858f66269d28c6dc2c80f6 --- /dev/null +++ b/lib/nmea/include/nmea/gmath.h @@ -0,0 +1,92 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_GMATH_H__ +#define __NMEA_GMATH_H__ + +#include "info.h" + +#define NMEA_PI (3.141592653589793) /**< PI value */ +#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ +#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ +#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ +#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ +#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ +#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ +#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * degree VS radian + */ + +double nmea_degree2radian(double val); +double nmea_radian2degree(double val); + +/* + * NDEG (NMEA degree) + */ + +double nmea_ndeg2degree(double val); +double nmea_degree2ndeg(double val); + +double nmea_ndeg2radian(double val); +double nmea_radian2ndeg(double val); + +/* + * DOP + */ + +double nmea_calc_pdop(double hdop, double vdop); +double nmea_dop2meters(double dop); +double nmea_meters2dop(double meters); + +/* + * positions work + */ + +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos); +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info); + +double nmea_distance( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos + ); + +double nmea_distance_ellipsoid( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos, + double *from_azimuth, + double *to_azimuth + ); + +int nmea_move_horz( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + double azimuth, + double distance + ); + +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + double azimuth, + double distance, + double *end_azimuth + ); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GMATH_H__ */ diff --git a/lib/nmea/include/nmea/info.h b/lib/nmea/include/nmea/info.h new file mode 100755 index 0000000000000000000000000000000000000000..46ec1cd522ec5f2f94353d2ea49682f934ce7d53 --- /dev/null +++ b/lib/nmea/include/nmea/info.h @@ -0,0 +1,112 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.h 10 2007-11-15 14:50:15Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_INFO_H__ +#define __NMEA_INFO_H__ + +#include "time.h" + +#define NMEA_SIG_BAD (0) +#define NMEA_SIG_LOW (1) +#define NMEA_SIG_MID (2) +#define NMEA_SIG_HIGH (3) + +#define NMEA_FIX_BAD (1) +#define NMEA_FIX_2D (2) +#define NMEA_FIX_3D (3) + +#define NMEA_MAXSAT (12) +#define NMEA_SATINPACK (4) +#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) + +#define NMEA_DEF_LAT (5001.2621) +#define NMEA_DEF_LON (3613.0595) + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Position data in fractional degrees or radians + */ +typedef struct _nmeaPOS +{ + double lat; /**< Latitude */ + double lon; /**< Longitude */ + +} nmeaPOS; + +/** + * Information about satellite + * @see nmeaSATINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATELLITE +{ + int id; /**< Satellite PRN number */ + int in_use; /**< Used in position fix */ + int elv; /**< Elevation in degrees, 90 maximum */ + int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ + int sig; /**< Signal, 00-99 dB */ + +} nmeaSATELLITE; + +/** + * Information about all satellites in view + * @see nmeaINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATINFO +{ + int inuse; /**< Number of satellites in use (not those in view) */ + int inview; /**< Total number of satellites in view */ + nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ + +} nmeaSATINFO; + +/** + * Summary GPS information from all parsed packets, + * used also for generating NMEA stream + * @see nmea_parse + * @see nmea_GPGGA2info, nmea_...2info + */ +typedef struct _nmeaINFO +{ + int smask; /**< Mask specifying types of packages from which data have been obtained */ + + nmeaTIME utc; /**< UTC of position */ + + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + + double PDOP; /**< Position Dilution Of Precision */ + double HDOP; /**< Horizontal Dilution Of Precision */ + double VDOP; /**< Vertical Dilution Of Precision */ + + double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ + double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ + double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ + double speed; /**< Speed over the ground in kilometers/hour */ + double direction; /**< Track angle in degrees True */ + double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + + nmeaSATINFO satinfo; /**< Satellites information */ + +} nmeaINFO; + +void nmea_zero_INFO(nmeaINFO *info); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_INFO_H__ */ diff --git a/lib/nmea/include/nmea/nmea.h b/lib/nmea/include/nmea/nmea.h new file mode 100755 index 0000000000000000000000000000000000000000..62692230f031134a13930eaf0ed1159284072611 --- /dev/null +++ b/lib/nmea/include/nmea/nmea.h @@ -0,0 +1,25 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: nmea.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_H__ +#define __NMEA_H__ + +#include "./config.h" +#include "./units.h" +#include "./gmath.h" +#include "./info.h" +#include "./sentence.h" +#include "./generate.h" +#include "./generator.h" +#include "./parse.h" +#include "./parser.h" +#include "./context.h" + +#endif /* __NMEA_H__ */ diff --git a/lib/nmea/include/nmea/parse.h b/lib/nmea/include/nmea/parse.h new file mode 100755 index 0000000000000000000000000000000000000000..3e6b425db70899ed346be0bc5f538d8947a64be4 --- /dev/null +++ b/lib/nmea/include/nmea/parse.h @@ -0,0 +1,39 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSE_H__ +#define __NMEA_PARSE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_pack_type(const char *buff, int buff_sz); +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc); + +int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack); +int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack); +int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack); +int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack); +int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack); + +void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info); +void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info); +void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info); +void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info); +void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSE_H__ */ diff --git a/lib/nmea/include/nmea/parser.h b/lib/nmea/include/nmea/parser.h new file mode 100755 index 0000000000000000000000000000000000000000..51a3fab7fd8290d472f8f0f4662c0b18e845361b --- /dev/null +++ b/lib/nmea/include/nmea/parser.h @@ -0,0 +1,59 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSER_H__ +#define __NMEA_PARSER_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +typedef struct _nmeaPARSER +{ + void *top_node; + void *end_node; + unsigned char *buffer; + int buff_size; + int buff_use; + +} nmeaPARSER; + +int nmea_parser_init(nmeaPARSER *parser); +void nmea_parser_destroy(nmeaPARSER *parser); + +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ); + +/* + * low level + */ + +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz); +int nmea_parser_top(nmeaPARSER *parser); +int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr); +int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr); +int nmea_parser_drop(nmeaPARSER *parser); +int nmea_parser_buff_clear(nmeaPARSER *parser); +int nmea_parser_queue_clear(nmeaPARSER *parser); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSER_H__ */ diff --git a/lib/nmea/include/nmea/sentence.h b/lib/nmea/include/nmea/sentence.h new file mode 100755 index 0000000000000000000000000000000000000000..412b0fe66535f78534dc3b671b64edbd1b973ce9 --- /dev/null +++ b/lib/nmea/include/nmea/sentence.h @@ -0,0 +1,128 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_SENTENCE_H__ +#define __NMEA_SENTENCE_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * NMEA packets type which parsed and generated by library + */ +enum nmeaPACKTYPE +{ + GPNON = 0x0000, /**< Unknown packet type. */ + GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ + GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ + GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ + GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ + GPVTG = 0x0010 /**< VTG - Actual track made good and speed over ground. */ +}; + +/** + * GGA packet information structure (Global Positioning System Fix Data) + */ +typedef struct _nmeaGPGGA +{ + nmeaTIME utc; /**< UTC of position (just time) */ + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int satinuse; /**< Number of satellites in use (not those in view) */ + double HDOP; /**< Horizontal dilution of precision */ + double elv; /**< Antenna altitude above/below mean sea level (geoid) */ + char elv_units; /**< [M]eters (Antenna height unit) */ + double diff; /**< Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. '-' = geoid is below WGS-84 ellipsoid) */ + char diff_units; /**< [M]eters (Units of geoidal separation) */ + double dgps_age; /**< Time in seconds since last DGPS update */ + int dgps_sid; /**< DGPS station ID number */ + +} nmeaGPGGA; + +/** + * GSA packet information structure (Satellite status) + */ +typedef struct _nmeaGPGSA +{ + char fix_mode; /**< Mode (M = Manual, forced to operate in 2D or 3D; A = Automatic, 3D/2D) */ + int fix_type; /**< Type, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + int sat_prn[NMEA_MAXSAT]; /**< PRNs of satellites used in position fix (null for unused fields) */ + double PDOP; /**< Dilution of precision */ + double HDOP; /**< Horizontal dilution of precision */ + double VDOP; /**< Vertical dilution of precision */ + +} nmeaGPGSA; + +/** + * GSV packet information structure (Satellites in view) + */ +typedef struct _nmeaGPGSV +{ + int pack_count; /**< Total number of messages of this type in this cycle */ + int pack_index; /**< Message number */ + int sat_count; /**< Total number of satellites in view */ + nmeaSATELLITE sat_data[NMEA_SATINPACK]; + +} nmeaGPGSV; + +/** + * RMC packet information structure (Recommended Minimum sentence C) + */ +typedef struct _nmeaGPRMC +{ + nmeaTIME utc; /**< UTC of position */ + char status; /**< Status (A = active or V = void) */ + double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + double speed; /**< Speed over the ground in knots */ + double direction; /**< Track angle in degrees True */ + double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + char declin_ew; /**< [E]ast or [W]est */ + char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ + +} nmeaGPRMC; + +/** + * VTG packet information structure (Track made good and ground speed) + */ +typedef struct _nmeaGPVTG +{ + double dir; /**< True track made good (degrees) */ + char dir_t; /**< Fixed text 'T' indicates that track made good is relative to true north */ + double dec; /**< Magnetic track made good */ + char dec_m; /**< Fixed text 'M' */ + double spn; /**< Ground speed, knots */ + char spn_n; /**< Fixed text 'N' indicates that speed over ground is in knots */ + double spk; /**< Ground speed, kilometers per hour */ + char spk_k; /**< Fixed text 'K' indicates that speed over ground is in kilometers/hour */ + +} nmeaGPVTG; + +void nmea_zero_GPGGA(nmeaGPGGA *pack); +void nmea_zero_GPGSA(nmeaGPGSA *pack); +void nmea_zero_GPGSV(nmeaGPGSV *pack); +void nmea_zero_GPRMC(nmeaGPRMC *pack); +void nmea_zero_GPVTG(nmeaGPVTG *pack); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_SENTENCE_H__ */ diff --git a/lib/nmea/include/nmea/time.h b/lib/nmea/include/nmea/time.h new file mode 100755 index 0000000000000000000000000000000000000000..bbe59f65c98227df789de7ef85c923a24042877e --- /dev/null +++ b/lib/nmea/include/nmea/time.h @@ -0,0 +1,47 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_TIME_H__ +#define __NMEA_TIME_H__ + +#include "config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Date and time data + * @see nmea_time_now + */ +typedef struct _nmeaTIME +{ + int year; /**< Years since 1900 */ + int mon; /**< Months since January - [0,11] */ + int day; /**< Day of the month - [1,31] */ + int hour; /**< Hours since midnight - [0,23] */ + int min; /**< Minutes after the hour - [0,59] */ + int sec; /**< Seconds after the minute - [0,59] */ + int hsec; /**< Hundredth part of second - [0,99] */ + +} nmeaTIME; + +/** + * \brief Get time now to nmeaTIME structure + */ +void nmea_time_now(nmeaTIME *t); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TIME_H__ */ diff --git a/lib/nmea/include/nmea/tok.h b/lib/nmea/include/nmea/tok.h new file mode 100755 index 0000000000000000000000000000000000000000..21557e52078d9703ef879c0599a189aa21e0b6eb --- /dev/null +++ b/lib/nmea/include/nmea/tok.h @@ -0,0 +1,30 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_TOK_H__ +#define __NMEA_TOK_H__ + +#include "config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_calc_crc(const char *buff, int buff_sz); +int nmea_atoi(const char *str, int str_sz, int radix); +double nmea_atof(const char *str, int str_sz); +int nmea_printf(char *buff, int buff_sz, const char *format, ...); +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TOK_H__ */ diff --git a/lib/nmea/include/nmea/units.h b/lib/nmea/include/nmea/units.h new file mode 100755 index 0000000000000000000000000000000000000000..767f980e2b026f34a6ef47165c409ae2d7e9e9c1 --- /dev/null +++ b/lib/nmea/include/nmea/units.h @@ -0,0 +1,30 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: units.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_UNITS_H__ +#define __NMEA_UNITS_H__ + +#include "config.h" + +/* + * Distance units + */ + +#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ +#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ +#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ + +/* + * Speed units + */ + +#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ + +#endif /* __NMEA_UNITS_H__ */ diff --git a/lib/nmea/info.c b/lib/nmea/info.c new file mode 100755 index 0000000000000000000000000000000000000000..1d531ffc4dda88e4f6d2b142acadb66fd106eeb2 --- /dev/null +++ b/lib/nmea/info.c @@ -0,0 +1,21 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include + +#include "nmea/info.h" + +void nmea_zero_INFO(nmeaINFO *info) +{ + memset(info, 0, sizeof(nmeaINFO)); + nmea_time_now(&info->utc); + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; +} diff --git a/lib/nmea/nmea.pri b/lib/nmea/nmea.pri new file mode 100644 index 0000000000000000000000000000000000000000..7bd78e7ab6346768a7939c4b1842e43f3a1d7bb9 --- /dev/null +++ b/lib/nmea/nmea.pri @@ -0,0 +1,23 @@ +DEPENDPATH += lib/nmea lib/nmea/include +INCLUDEPATH += lib/nmea/include + +# Input +HEADERS += include/nmea/config.h \ + include/nmea/context.h \ + include/nmea/gmath.h \ + include/nmea/info.h \ + include/nmea/nmea.h \ + include/nmea/parse.h \ + include/nmea/parser.h \ + include/nmea/sentence.h \ + include/nmea/time.h \ + include/nmea/tok.h \ + include/nmea/units.h +SOURCES += context.c \ + gmath.c \ + info.c \ + parse.c \ + parser.c \ + sentence.c \ + time.c \ + tok.c diff --git a/lib/nmea/parse.c b/lib/nmea/parse.c new file mode 100755 index 0000000000000000000000000000000000000000..7a476dc75bb0fa85acf817633a95eec63b37d8db --- /dev/null +++ b/lib/nmea/parse.c @@ -0,0 +1,501 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parse.h + * \brief Functions of a low level for analysis of + * packages of NMEA stream. + * + * \code + * ... + * ptype = nmea_pack_type( + * (const char *)parser->buffer + nparsed + 1, + * parser->buff_use - nparsed - 1); + * + * if(0 == (node = malloc(sizeof(nmeaParserNODE)))) + * goto mem_fail; + * + * node->pack = 0; + * + * switch(ptype) + * { + * case GPGGA: + * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) + * goto mem_fail; + * node->packType = GPGGA; + * if(!nmea_parse_GPGGA( + * (const char *)parser->buffer + nparsed, + * sen_sz, (nmeaGPGGA *)node->pack)) + * { + * free(node); + * node = 0; + * } + * break; + * case GPGSA: + * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) + * goto mem_fail; + * node->packType = GPGSA; + * if(!nmea_parse_GPGSA( + * (const char *)parser->buffer + nparsed, + * sen_sz, (nmeaGPGSA *)node->pack)) + * { + * free(node); + * node = 0; + * } + * break; + * ... + * \endcode + */ + +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/context.h" +#include "nmea/gmath.h" +#include "nmea/units.h" + +#include +#include + +int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res) +{ + int success = 0; + + switch(buff_sz) + { + case sizeof("hhmmss") - 1: + success = (3 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec) + )); + break; + case sizeof("hhmmss.s") - 1: + case sizeof("hhmmss.ss") - 1: + case sizeof("hhmmss.sss") - 1: + success = (4 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec) + )); + break; + default: + nmea_error("Parse of time error (format error)!"); + success = 0; + break; + } + + return (success?0:-1); +} + +/** + * \brief Define packet type by header (nmeaPACKTYPE). + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @return The defined packet type + * @see nmeaPACKTYPE + */ +int nmea_pack_type(const char *buff, int buff_sz) +{ + static const char *pheads[] = { + "GPGGA", + "GPGSA", + "GPGSV", + "GPRMC", + "GPVTG", + }; + + NMEA_ASSERT(buff); + + if(buff_sz < 5) + return GPNON; + else if(0 == memcmp(buff, pheads[0], 5)) + return GPGGA; + else if(0 == memcmp(buff, pheads[1], 5)) + return GPGSA; + else if(0 == memcmp(buff, pheads[2], 5)) + return GPGSV; + else if(0 == memcmp(buff, pheads[3], 5)) + return GPRMC; + else if(0 == memcmp(buff, pheads[4], 5)) + return GPVTG; + + return GPNON; +} + +/** + * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC). + * @param buff a constant character pointer of packets buffer. + * @param buff_sz buffer size. + * @param res_crc a integer pointer for return CRC of packet (must be defined). + * @return Number of bytes to packet tail. + */ +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) +{ + static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; + + const char *end_buff = buff + buff_sz; + int nread = 0; + int crc = 0; + + NMEA_ASSERT(buff && res_crc); + + *res_crc = -1; + + for(;buff < end_buff; ++buff, ++nread) + { + if(('$' == *buff) && nread) + { + buff = 0; + break; + } + else if('*' == *buff) + { + if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) + { + *res_crc = nmea_atoi(buff + 1, 2, 16); + nread = buff_sz - (int)(end_buff - (buff + tail_sz)); + if(*res_crc != crc) + { + *res_crc = -1; + buff = 0; + } + } + + break; + } + else if(nread) + crc ^= (int)*buff; + } + + if(*res_crc < 0 && buff) + nread = 0; + + return nread; +} + +/** + * \brief Parse GGA packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack) +{ + char time_buff[NMEA_TIMEPARSE_BUF]; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGGA)); + + nmea_trace_buff(buff, buff_sz); + + if(14 != nmea_scanf(buff, buff_sz, + "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*", + &(time_buff[0]), + &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units), + &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid))) + { + nmea_error("GPGGA parse error!"); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("GPGGA time parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse GSA packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack) +{ + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGSA)); + + nmea_trace_buff(buff, buff_sz); + + if(17 != nmea_scanf(buff, buff_sz, + "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*", + &(pack->fix_mode), &(pack->fix_type), + &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]), + &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]), + &(pack->PDOP), &(pack->HDOP), &(pack->VDOP))) + { + nmea_error("GPGSA parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse GSV packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack) +{ + int nsen, nsat; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGSV)); + + nmea_trace_buff(buff, buff_sz); + + nsen = nmea_scanf(buff, buff_sz, + "$GPGSV,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d*", + &(pack->pack_count), &(pack->pack_index), &(pack->sat_count), + &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig), + &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig), + &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig), + &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig)); + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + nsat = nsat * 4 + 3 /* first three sentence`s */; + + if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3)) + { + nmea_error("GPGSV parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse RMC packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack) +{ + int nsen; + char time_buff[NMEA_TIMEPARSE_BUF]; + + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPRMC)); + + nmea_trace_buff(buff, buff_sz); + + nsen = nmea_scanf(buff, buff_sz, + "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*", + &(time_buff[0]), + &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->speed), &(pack->direction), + &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), + &(pack->declination), &(pack->declin_ew), &(pack->mode)); + + if(nsen != 13 && nsen != 14) + { + nmea_error("GPRMC parse error!"); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("GPRMC time parse error!"); + return 0; + } + + if(pack->utc.year < 90) + pack->utc.year += 100; + pack->utc.mon -= 1; + + return 1; +} + +/** + * \brief Parse VTG packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack) +{ + NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPVTG)); + + nmea_trace_buff(buff, buff_sz); + + if(8 != nmea_scanf(buff, buff_sz, + "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*", + &(pack->dir), &(pack->dir_t), + &(pack->dec), &(pack->dec_m), + &(pack->spn), &(pack->spn_n), + &(pack->spk), &(pack->spk_k))) + { + nmea_error("GPVTG parse error!"); + return 0; + } + + if( pack->dir_t != 'T' || + pack->dec_m != 'M' || + pack->spn_n != 'N' || + pack->spk_k != 'K') + { + nmea_error("GPVTG parse error (format error)!"); + return 0; + } + + return 1; +} + +/** + * \brief Fill nmeaINFO structure by GGA packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info) +{ + NMEA_ASSERT(pack && info); + + info->utc.hour = pack->utc.hour; + info->utc.min = pack->utc.min; + info->utc.sec = pack->utc.sec; + info->utc.hsec = pack->utc.hsec; + info->sig = pack->sig; + info->HDOP = pack->HDOP; + info->elv = pack->elv; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); + info->smask |= GPGGA; +} + +/** + * \brief Fill nmeaINFO structure by GSA packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info) +{ + int i, j, nuse = 0; + + NMEA_ASSERT(pack && info); + + info->fix = pack->fix_type; + info->PDOP = pack->PDOP; + info->HDOP = pack->HDOP; + info->VDOP = pack->VDOP; + + for(i = 0; i < NMEA_MAXSAT; ++i) + { + for(j = 0; j < info->satinfo.inview; ++j) + { + if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id) + { + info->satinfo.sat[j].in_use = 1; + nuse++; + } + } + } + + info->satinfo.inuse = nuse; + info->smask |= GPGSA; +} + +/** + * \brief Fill nmeaINFO structure by GSV packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info) +{ + int isat, isi, nsat; + + NMEA_ASSERT(pack && info); + + if(pack->pack_index > pack->pack_count || + pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT) + return; + + if(pack->pack_index < 1) + pack->pack_index = 1; + + info->satinfo.inview = pack->sat_count; + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + + for(isat = 0; isat < nsat; ++isat) + { + isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat; + info->satinfo.sat[isi].id = pack->sat_data[isat].id; + info->satinfo.sat[isi].elv = pack->sat_data[isat].elv; + info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth; + info->satinfo.sat[isi].sig = pack->sat_data[isat].sig; + } + + info->smask |= GPGSV; +} + +/** + * \brief Fill nmeaINFO structure by RMC packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info) +{ + NMEA_ASSERT(pack && info); + + if('A' == pack->status) + { + if(NMEA_SIG_BAD == info->sig) + info->sig = NMEA_SIG_MID; + if(NMEA_FIX_BAD == info->fix) + info->fix = NMEA_FIX_2D; + } + else if('V' == pack->status) + { + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; + } + + info->utc = pack->utc; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); + info->speed = pack->speed * NMEA_TUD_KNOTS; + info->direction = pack->direction; + info->smask |= GPRMC; +} + +/** + * \brief Fill nmeaINFO structure by VTG packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info) +{ + NMEA_ASSERT(pack && info); + + info->direction = pack->dir; + info->declination = pack->dec; + info->speed = pack->spk; + info->smask |= GPVTG; +} diff --git a/lib/nmea/parser.c b/lib/nmea/parser.c new file mode 100755 index 0000000000000000000000000000000000000000..b973853bf6ed26b07bd5e01e88fc75a9891d4e5f --- /dev/null +++ b/lib/nmea/parser.c @@ -0,0 +1,401 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parser.h + */ + +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/parser.h" +#include "nmea/context.h" + +#include +#include + +typedef struct _nmeaParserNODE +{ + int packType; + void *pack; + struct _nmeaParserNODE *next_node; + +} nmeaParserNODE; + +/* + * high level + */ + +/** + * \brief Initialization of parser object + * @return true (1) - success or false (0) - fail + */ +int nmea_parser_init(nmeaPARSER *parser) +{ + int resv = 0; + int buff_size = nmea_property()->parse_buff_size; + + NMEA_ASSERT(parser); + + if(buff_size < NMEA_MIN_PARSEBUFF) + buff_size = NMEA_MIN_PARSEBUFF; + + memset(parser, 0, sizeof(nmeaPARSER)); + + if(0 == (parser->buffer = malloc(buff_size))) + nmea_error("Insufficient memory!"); + else + { + parser->buff_size = buff_size; + resv = 1; + } + + return resv; +} + +/** + * \brief Destroy parser object + */ +void nmea_parser_destroy(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser && parser->buffer); + free(parser->buffer); + nmea_parser_queue_clear(parser); + memset(parser, 0, sizeof(nmeaPARSER)); +} + +/** + * \brief Analysis of buffer and put results to information structure + * @return Number of packets wos parsed + */ +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ) +{ + int ptype, nread = 0; + void *pack = 0; + + NMEA_ASSERT(parser && parser->buffer); + + nmea_parser_push(parser, buff, buff_sz); + + while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) + { + nread++; + + switch(ptype) + { + case GPGGA: + nmea_GPGGA2info((nmeaGPGGA *)pack, info); + break; + case GPGSA: + nmea_GPGSA2info((nmeaGPGSA *)pack, info); + break; + case GPGSV: + nmea_GPGSV2info((nmeaGPGSV *)pack, info); + break; + case GPRMC: + nmea_GPRMC2info((nmeaGPRMC *)pack, info); + break; + case GPVTG: + nmea_GPVTG2info((nmeaGPVTG *)pack, info); + break; + }; + + free(pack); + } + + return nread; +} + +/* + * low level + */ + +int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparsed = 0, crc, sen_sz, ptype; + nmeaParserNODE *node = 0; + + NMEA_ASSERT(parser && parser->buffer); + + /* clear unuse buffer (for debug) */ + /* + memset( + parser->buffer + parser->buff_use, 0, + parser->buff_size - parser->buff_use + ); + */ + + /* add */ + if(parser->buff_use + buff_sz >= parser->buff_size) + nmea_parser_buff_clear(parser); + + memcpy(parser->buffer + parser->buff_use, buff, buff_sz); + parser->buff_use += buff_sz; + + /* parse */ + for(;;node = 0) + { + sen_sz = nmea_find_tail( + (const char *)parser->buffer + nparsed, + (int)parser->buff_use - nparsed, &crc); + + if(!sen_sz) + { + if(nparsed) + memcpy( + parser->buffer, + parser->buffer + nparsed, + parser->buff_use -= nparsed); + break; + } + else if(crc >= 0) + { + ptype = nmea_pack_type( + (const char *)parser->buffer + nparsed + 1, + parser->buff_use - nparsed - 1); + + if(0 == (node = malloc(sizeof(nmeaParserNODE)))) + goto mem_fail; + + node->pack = 0; + + switch(ptype) + { + case GPGGA: + if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) + goto mem_fail; + node->packType = GPGGA; + if(!nmea_parse_GPGGA( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGGA *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPGSA: + if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) + goto mem_fail; + node->packType = GPGSA; + if(!nmea_parse_GPGSA( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGSA *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPGSV: + if(0 == (node->pack = malloc(sizeof(nmeaGPGSV)))) + goto mem_fail; + node->packType = GPGSV; + if(!nmea_parse_GPGSV( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGSV *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPRMC: + if(0 == (node->pack = malloc(sizeof(nmeaGPRMC)))) + goto mem_fail; + node->packType = GPRMC; + if(!nmea_parse_GPRMC( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPRMC *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPVTG: + if(0 == (node->pack = malloc(sizeof(nmeaGPVTG)))) + goto mem_fail; + node->packType = GPVTG; + if(!nmea_parse_GPVTG( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPVTG *)node->pack)) + { + free(node); + node = 0; + } + break; + default: + free(node); + node = 0; + break; + }; + + if(node) + { + if(parser->end_node) + ((nmeaParserNODE *)parser->end_node)->next_node = node; + parser->end_node = node; + if(!parser->top_node) + parser->top_node = node; + node->next_node = 0; + } + } + + nparsed += sen_sz; + } + + return nparsed; + +mem_fail: + if(node) + free(node); + + nmea_error("Insufficient memory!"); + + return -1; +} + +/** + * \brief Analysis of buffer and keep results into parser + * @return Number of bytes wos parsed from buffer + */ +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparse, nparsed = 0; + + do + { + if(buff_sz > parser->buff_size) + nparse = parser->buff_size; + else + nparse = buff_sz; + + nparsed += nmea_parser_real_push( + parser, buff, nparse); + + buff_sz -= nparse; + + } while(buff_sz); + + return nparsed; +} + +/** + * \brief Get type of top packet keeped into parser + * @return Type of packet + * @see nmeaPACKTYPE + */ +int nmea_parser_top(nmeaPARSER *parser) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + retval = node->packType; + + return retval; +} + +/** + * \brief Withdraw top packet from parser + * @return Received packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + retval = node->packType; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return retval; +} + +/** + * \brief Get top packet from parser without withdraw + * @return Received packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + retval = node->packType; + } + + return retval; +} + +/** + * \brief Delete top packet from parser + * @return Deleted packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_drop(nmeaPARSER *parser) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + if(node->pack) + free(node->pack); + retval = node->packType; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return retval; +} + +/** + * \brief Clear cache of parser + * @return true (1) - success + */ +int nmea_parser_buff_clear(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser && parser->buffer); + parser->buff_use = 0; + return 1; +} + +/** + * \brief Clear packets queue into parser + * @return true (1) - success + */ +int nmea_parser_queue_clear(nmeaPARSER *parser) +{ + NMEA_ASSERT(parser); + while(parser->top_node) + nmea_parser_drop(parser); + return 1; +} diff --git a/lib/nmea/sentence.c b/lib/nmea/sentence.c new file mode 100755 index 0000000000000000000000000000000000000000..a66393a4da4976a6f81ca95ed4c62ce892b21ca6 --- /dev/null +++ b/lib/nmea/sentence.c @@ -0,0 +1,54 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/sentence.h" + +#include + +void nmea_zero_GPGGA(nmeaGPGGA *pack) +{ + memset(pack, 0, sizeof(nmeaGPGGA)); + nmea_time_now(&pack->utc); + pack->ns = 'N'; + pack->ew = 'E'; + pack->elv_units = 'M'; + pack->diff_units = 'M'; +} + +void nmea_zero_GPGSA(nmeaGPGSA *pack) +{ + memset(pack, 0, sizeof(nmeaGPGSA)); + pack->fix_mode = 'A'; + pack->fix_type = NMEA_FIX_BAD; +} + +void nmea_zero_GPGSV(nmeaGPGSV *pack) +{ + memset(pack, 0, sizeof(nmeaGPGSV)); +} + +void nmea_zero_GPRMC(nmeaGPRMC *pack) +{ + memset(pack, 0, sizeof(nmeaGPRMC)); + nmea_time_now(&pack->utc); + pack->status = 'V'; + pack->ns = 'N'; + pack->ew = 'E'; + pack->declin_ew = 'E'; +} + +void nmea_zero_GPVTG(nmeaGPVTG *pack) +{ + memset(pack, 0, sizeof(nmeaGPVTG)); + pack->dir_t = 'T'; + pack->dec_m = 'M'; + pack->spn_n = 'N'; + pack->spk_k = 'K'; +} diff --git a/lib/nmea/time.c b/lib/nmea/time.c new file mode 100755 index 0000000000000000000000000000000000000000..a2431963018e5382555789ad407e89ef4f55c028 --- /dev/null +++ b/lib/nmea/time.c @@ -0,0 +1,63 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.c 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file time.h */ + +#include "nmea/time.h" + +#ifdef NMEA_WIN +# pragma warning(disable: 4201) +# pragma warning(disable: 4214) +# pragma warning(disable: 4115) +# include +# pragma warning(default: 4201) +# pragma warning(default: 4214) +# pragma warning(default: 4115) +#else +# include +#endif + +#ifdef NMEA_WIN + +void nmea_time_now(nmeaTIME *stm) +{ + SYSTEMTIME st; + + GetSystemTime(&st); + + stm->year = st.wYear - 1900; + stm->mon = st.wMonth - 1; + stm->day = st.wDay; + stm->hour = st.wHour; + stm->min = st.wMinute; + stm->sec = st.wSecond; + stm->hsec = st.wMilliseconds / 10; +} + +#else /* NMEA_WIN */ + +void nmea_time_now(nmeaTIME *stm) +{ + time_t lt; + struct tm *tt; + + time(<); + tt = gmtime(<); + + stm->year = tt->tm_year; + stm->mon = tt->tm_mon; + stm->day = tt->tm_mday; + stm->hour = tt->tm_hour; + stm->min = tt->tm_min; + stm->sec = tt->tm_sec; + stm->hsec = 0; +} + +#endif diff --git a/lib/nmea/tok.c b/lib/nmea/tok.c new file mode 100755 index 0000000000000000000000000000000000000000..585bdefbe3fb67f1ee54dae0073717743c94f92c --- /dev/null +++ b/lib/nmea/tok.c @@ -0,0 +1,250 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file tok.h */ + +#include "nmea/tok.h" + +#include +#include +#include +#include +#include +#include + +#define NMEA_TOKS_COMPARE (1) +#define NMEA_TOKS_PERCENT (2) +#define NMEA_TOKS_WIDTH (3) +#define NMEA_TOKS_TYPE (4) + +/** + * \brief Calculate control sum of binary buffer + */ +int nmea_calc_crc(const char *buff, int buff_sz) +{ + int chsum = 0, + it; + + for(it = 0; it < buff_sz; ++it) + chsum ^= (int)buff[it]; + + return chsum; +} + +/** + * \brief Convert string to number + */ +int nmea_atoi(const char *str, int str_sz, int radix) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + int res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = strtol(&buff[0], &tmp_ptr, radix); + } + + return res; +} + +/** + * \brief Convert string to fraction number + */ +double nmea_atof(const char *str, int str_sz) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + double res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = strtod(&buff[0], &tmp_ptr); + } + + return res; +} + +/** + * \brief Formating string (like standart printf) with CRC tail (*CRC) + */ +int nmea_printf(char *buff, int buff_sz, const char *format, ...) +{ + int retval, add = 0; + va_list arg_ptr; + + if(buff_sz <= 0) + return 0; + + va_start(arg_ptr, format); + + retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); + + if(retval > 0) + { + add = NMEA_POSIX(snprintf)( + buff + retval, buff_sz - retval, "*%02x\r\n", + nmea_calc_crc(buff + 1, retval - 1)); + } + + retval += add; + + if(retval < 0 || retval > buff_sz) + { + memset(buff, ' ', buff_sz); + retval = buff_sz; + } + + va_end(arg_ptr); + + return retval; +} + +/** + * \brief Analyse string (specificate for NMEA sentences) + */ +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...) +{ + const char *beg_tok; + const char *end_buf = buff + buff_sz; + + va_list arg_ptr; + int tok_type = NMEA_TOKS_COMPARE; + int width = 0; + const char *beg_fmt = 0; + int snum = 0, unum = 0; + + int tok_count = 0; + void *parg_target; + + va_start(arg_ptr, format); + + for(; *format && buff < end_buf; ++format) + { + switch(tok_type) + { + case NMEA_TOKS_COMPARE: + if('%' == *format) + tok_type = NMEA_TOKS_PERCENT; + else if(*buff++ != *format) + goto fail; + break; + case NMEA_TOKS_PERCENT: + width = 0; + beg_fmt = format; + tok_type = NMEA_TOKS_WIDTH; + case NMEA_TOKS_WIDTH: + if(isdigit(*format)) + break; + { + tok_type = NMEA_TOKS_TYPE; + if(format > beg_fmt) + width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10); + } + case NMEA_TOKS_TYPE: + beg_tok = buff; + + if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) + width = 1; + + if(width) + { + if(buff + width <= end_buf) + buff += width; + else + goto fail; + } + else + { + if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) + buff = end_buf; + } + + if(buff > end_buf) + goto fail; + + tok_type = NMEA_TOKS_COMPARE; + tok_count++; + + parg_target = 0; width = (int)(buff - beg_tok); + + switch(*format) + { + case 'c': + case 'C': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + *((char *)parg_target) = *beg_tok; + break; + case 's': + case 'S': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + { + memcpy(parg_target, beg_tok, width); + ((char *)parg_target)[width] = '\0'; + } + break; + case 'f': + case 'g': + case 'G': + case 'e': + case 'E': + parg_target = (void *)va_arg(arg_ptr, double *); + if(width && 0 != (parg_target)) + *((double *)parg_target) = nmea_atof(beg_tok, width); + break; + }; + + if(parg_target) + break; + if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) + break; + if(!width) + break; + + switch(*format) + { + case 'd': + case 'i': + snum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &snum, sizeof(int)); + break; + case 'u': + unum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'x': + case 'X': + unum = nmea_atoi(beg_tok, width, 16); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'o': + unum = nmea_atoi(beg_tok, width, 8); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + default: + goto fail; + }; + + break; + }; + } + +fail: + + va_end(arg_ptr); + + return tok_count; +} diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index f846c60db2671460785bc69f416384ec14913d81..2657c46cd0501883005530386eb51391671f1c6c 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -38,6 +38,9 @@ release { QMAKE_POST_LINK += echo "Copying files" +# Turn off serial port warnings +DEFINES += _TTY_NOWARN_ + #QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/. #QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 13ff2dc06f452008305730b463382a5dcd452e27..a260a4390a813dcc8ae527f70440cde20e317221 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -25,6 +25,7 @@ # Include bundled version if necessary include(lib/QMapControl/QMapControl.pri) +include(lib/nmea/nmea.pri) # message("Including bundled QMapControl version as FALLBACK. This is fine on Linux and MacOS, but not the best choice in Windows") QT += network \ @@ -140,7 +141,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 \ @@ -154,7 +154,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/designer/QGCToolWidget.ui \ src/ui/designer/QGCParamSlider.ui \ src/ui/designer/QGCActionButton.ui \ - src/ui/QGCMAVLinkLogPlayer.ui + src/ui/QGCMAVLinkLogPlayer.ui \ + src/ui/QGCWaypointListMulti.ui INCLUDEPATH += src \ src/ui \ @@ -263,7 +264,8 @@ HEADERS += src/MG.h \ src/ui/QGCMAVLinkLogPlayer.h \ src/comm/MAVLinkSimulationWaypointPlanner.h \ src/comm/MAVLinkSimulationMAV.h \ - src/uas/QGCMAVLinkUASFactory.h + src/uas/QGCMAVLinkUASFactory.h \ + src/ui/QGCWaypointListMulti.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008: { @@ -388,6 +390,7 @@ SOURCES += src/main.cc \ src/comm/MAVLinkSimulationWaypointPlanner.cc \ src/comm/MAVLinkSimulationMAV.cc \ src/uas/QGCMAVLinkUASFactory.cc \ + src/ui/QGCWaypointListMulti.cc macx|win32-msvc2008: { SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/QGC.cc b/src/QGC.cc index 087c4885742a48ca3efdc1ad61705f1ea64e4701..04735eb1f3df10553f6646efe151b66a08bc55b7 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -23,6 +23,8 @@ This file is part of the QGROUNDCONTROL project #include "QGC.h" +#include + namespace QGC { quint64 groundTimeUsecs() @@ -34,6 +36,25 @@ quint64 groundTimeUsecs() return static_cast(microseconds + (time.time().msec()*1000)); } +double limitAngleToPMPI(double angle) +{ + if (angle < -M_PI) + { + while (angle < -M_PI) + { + angle += M_PI; + } + } + else if (angle > M_PI) + { + while (angle > M_PI) + { + angle -= M_PI; + } + } + return angle; +} + int applicationVersion() { return APPLICATIONVERSION; diff --git a/src/QGC.h b/src/QGC.h index 7ece2f230415217e17e70a65c24c0907801315ed..b777115a92c845a5e58939bd5bc90ba03041ca6b 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -18,6 +18,8 @@ namespace QGC /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); + /** @brief Returns the angle limited to -pi - pi */ + double limitAngleToPMPI(double angle); int applicationVersion(); const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21; 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/LinkInterface.h b/src/comm/LinkInterface.h index 21efec7a0dbf9bc15d99130d4ab86ee10f8b5fd4..3deea5aca5123aa17ed5fc0fe8ea610e2ba7a37a 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -228,6 +228,9 @@ signals: */ void nameChanged(QString name); + /** @brief Communication error occured */ + void communicationError(const QString& linkname, const QString& error); + protected: static int getNextLinkId() { diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 57ac970bb25f97bb66849b72fdc0fca32008dafa..32c9b500585613f1ab7c07eedf20f43b24f8b779 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -77,10 +77,7 @@ MAVLinkProtocol::~MAVLinkProtocol() void MAVLinkProtocol::run() { - forever - { - QGC::SLEEP::msleep(5000); - } + exec(); } QString MAVLinkProtocol::getLogfileName() diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 350cba49145db9df798c7ba02d14790ae095afd2..0eb07475bc519fd7251697d35f63d4ac8a89b1cd 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) @@ -444,12 +444,17 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; - // GLOBAL POSITION VEHICLE 2 - mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; +// // GLOBAL POSITION VEHICLE 2 +// mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed); +// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); +// //add data into datastream +// 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); @@ -624,15 +629,15 @@ void MAVLinkSimulationLink::mainloop() - // HEARTBEAT VEHICLE 2 +// // 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); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; +// // Pack message and get size of encoded byte string +// 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 +// memcpy(stream+streampointer,buffer, bufferlength); +// streampointer += bufferlength; // // HEARTBEAT VEHICLE 3 @@ -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++) { @@ -952,8 +956,10 @@ bool MAVLinkSimulationLink::connect() emit connected(true); start(LowPriority); - MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1); + MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 47.376, 8.548); + MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2); Q_UNUSED(mav1); + Q_UNUSED(mav2); // timer->start(rate); return true; } diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 6f9b7b9e639ea307856f7c4891fd9a72f1a65cc4..8f8e8eebbe224ac952ad8bd2dbf4eca70e2495a5 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -3,33 +3,37 @@ #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) +MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon) : + QObject(parent), + link(parent), + planner(parent, systemid), + systemid(systemid), + timer25Hz(0), + timer10Hz(0), + timer1Hz(0), + latitude(lat), + longitude(lon), + altitude(0.0), + x(lon), + y(lat), + 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..3a9d37adaec3db61513cf332de520b04f76fb2cd 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -11,7 +11,7 @@ class MAVLinkSimulationMAV : public QObject { Q_OBJECT public: - explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid); + explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056); signals: @@ -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/comm/QGCNMEAProtocol.cc b/src/comm/QGCNMEAProtocol.cc new file mode 100644 index 0000000000000000000000000000000000000000..45ac96fb436b186d887569c1b1f1f34d036d6908 --- /dev/null +++ b/src/comm/QGCNMEAProtocol.cc @@ -0,0 +1,14 @@ +/** + * The bytes are copied by calling the LinkInterface::readBytes() method. + * This method parses all incoming bytes and decodes GPS positions. + * @param link The interface to read from + * @see LinkInterface + **/ +void QGCNMEAProtocol::receiveBytes(LinkInterface* link, QByteArray b) +{ + receiveMutex.lock(); + + + + receiveMutex.unlock(); +} diff --git a/src/comm/QGCNMEAProtocol.h b/src/comm/QGCNMEAProtocol.h new file mode 100644 index 0000000000000000000000000000000000000000..0e19973d9708e91c3f3f4db54c7d3c2b35559c50 --- /dev/null +++ b/src/comm/QGCNMEAProtocol.h @@ -0,0 +1,75 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of class QGCNMEAProtocol + * @author Lorenz Meier + */ + +#ifndef QGCNMEAPROTOCOL_H_ +#define QGCNMEAPROTOCOL_H_ + +#include +#include +#include +#include +#include +#include "nmea/parse.h" +#include "ProtocolInterface.h" +#include "LinkInterface.h" + +/** + * @brief MAVLink micro air vehicle protocol reference implementation. + * + * MAVLink is a generic communication protocol for micro air vehicles. + * for more information, please see the official website. + * @ref http://pixhawk.ethz.ch/software/mavlink/ + **/ +class QGCNMEAProtocol : public ProtocolInterface { + Q_OBJECT + +public: + QGCNMEAProtocol(); + ~QGCNMEAProtocol(); + + void run(); + /** @brief Get the human-friendly name of this protocol */ + QString getName() { return QString("NMEA (GPS)"); } + +public slots: + /** @brief Receive bytes from a communication interface */ + void receiveBytes(LinkInterface* link, QByteArray b); + +protected: + QMutex receiveMutex; ///< Mutex to protect receiveBytes function + QMap + +signals: + /** @brief Message received and directly copied via signal */ + void positionReceived(double lat, double lon, double alt); + /** @brief Emitted if a message from the protocol should reach the user */ + void protocolStatusMessage(const QString& title, const QString& message); +}; + +#endif // QGCNMEAPROTOCOL_H_ diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index b3d88c30097ae7d7df787ab567f8345fa0865e94..0420dbaae4c9b15b72deccaa9fa822cd27a8ff78 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -175,18 +175,29 @@ void SerialLink::writeBytes(const char* data, qint64 size) if(port && port->isOpen()) { int b = port->write(data, size); - qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; - // Increase write counter - bitsSentTotal += size * 8; + if (b > 0) + { + +// qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; - int i; - for (i=0; igetName(), tr("Could not send data - link %1 is disconnected!").arg(this->getName())); } - qDebug("\n"); } } diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 088487043e019aa61bce853efa5d9a8852b112b5..3c3240498e88aa37973cc056e29043c12805e7f5 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -64,10 +64,11 @@ UDPLink::~UDPLink() **/ void UDPLink::run() { - forever - { - QGC::SLEEP::msleep(5000); - } +// forever +// { +// QGC::SLEEP::msleep(5000); +// } + exec(); } void UDPLink::setAddress(QString address) diff --git a/src/configuration.h b/src/configuration.h index c91fae7efbcffd00cab6f9ef7ecefedc5c0fa230..51dec3d2b83e8f2472a2b40dd4a05e792fa46465 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -17,14 +17,14 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.8.1 (Alpha)" +#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC6)" namespace QGC { const QString APPNAME = "QGROUNDCONTROL"; const QString COMPANYNAME = "OPENMAV"; - const int APPLICATIONVERSION = 80; // 0.8.0 + const int APPLICATIONVERSION = 83; // 0.8.0 } #endif // CONFIGURATION_H diff --git a/src/lib/qextserialport/posix_qextserialport.cpp b/src/lib/qextserialport/posix_qextserialport.cpp index 99247130b041c23ef84584331203465e984ffe2d..b6f70926009a8426d1c1c70110f61a431c237f02 100644 --- a/src/lib/qextserialport/posix_qextserialport.cpp +++ b/src/lib/qextserialport/posix_qextserialport.cpp @@ -1131,7 +1131,9 @@ qint64 Posix_QextSerialPort::writeData(const char * data, qint64 maxSize) int retVal = 0; retVal = ::write(fd, data, maxSize); if (retVal == -1) + { lastErr = E_WRITE_FAILED; + } UNLOCK_MUTEX(); return (qint64)retVal; diff --git a/src/lib/qmapcontrol/qmapcontrol.pri b/src/lib/qmapcontrol/qmapcontrol.pri deleted file mode 100644 index f03639792dfc8fa4de8d83fc54e1a439ccb02d56..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/qmapcontrol.pri +++ /dev/null @@ -1,52 +0,0 @@ -DEPENDPATH += src -INCLUDEPATH += src - -# Input -HEADERS += curve.h \ - geometry.h \ - imagemanager.h \ - layer.h \ - layermanager.h \ - linestring.h \ - mapadapter.h \ - mapcontrol.h \ - mapnetwork.h \ - point.h \ - tilemapadapter.h \ - wmsmapadapter.h \ - circlepoint.h \ - imagepoint.h \ - gps_position.h \ - osmmapadapter.h \ - maplayer.h \ - geometrylayer.h \ - yahoomapadapter.h \ - googlemapadapter.h \ - googlesatmapadapter.h \ - openaerialmapadapter.h \ - fixedimageoverlay.h \ - emptymapadapter.h -SOURCES += curve.cpp \ - geometry.cpp \ - imagemanager.cpp \ - layer.cpp \ - layermanager.cpp \ - linestring.cpp \ - mapadapter.cpp \ - mapcontrol.cpp \ - mapnetwork.cpp \ - point.cpp \ - tilemapadapter.cpp \ - wmsmapadapter.cpp \ - circlepoint.cpp \ - imagepoint.cpp \ - gps_position.cpp \ - osmmapadapter.cpp \ - maplayer.cpp \ - geometrylayer.cpp \ - yahoomapadapter.cpp \ - googlemapadapter.cpp \ - googlesatmapadapter.cpp \ - openaerialmapadapter.cpp \ - fixedimageoverlay.cpp \ - emptymapadapter.cpp diff --git a/src/lib/qmapcontrol/src/Doxyfile b/src/lib/qmapcontrol/src/Doxyfile deleted file mode 100644 index ba69d85e6f7d9abbd303e35c854078dca40eed29..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/Doxyfile +++ /dev/null @@ -1,283 +0,0 @@ -# Doxyfile 1.5.1-KDevelop - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- -PROJECT_NAME = QMapControl -PROJECT_NUMBER = 1 -OUTPUT_DIRECTORY = -CREATE_SUBDIRS = NO -OUTPUT_LANGUAGE = English -USE_WINDOWS_ENCODING = NO -BRIEF_MEMBER_DESC = YES -REPEAT_BRIEF = YES -ABBREVIATE_BRIEF = "The $name class" \ - "The $name widget" \ - "The $name file" \ - is \ - provides \ - specifies \ - contains \ - represents \ - a \ - an \ - the -ALWAYS_DETAILED_SEC = NO -INLINE_INHERITED_MEMB = NO -FULL_PATH_NAMES = YES -STRIP_FROM_PATH = /home/kai/ -STRIP_FROM_INC_PATH = -SHORT_NAMES = NO -JAVADOC_AUTOBRIEF = NO -MULTILINE_CPP_IS_BRIEF = NO -DETAILS_AT_TOP = NO -INHERIT_DOCS = YES -SEPARATE_MEMBER_PAGES = NO -TAB_SIZE = 8 -ALIASES = -OPTIMIZE_OUTPUT_FOR_C = NO -OPTIMIZE_OUTPUT_JAVA = NO -BUILTIN_STL_SUPPORT = NO -DISTRIBUTE_GROUP_DOC = NO -SUBGROUPING = YES -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- -EXTRACT_ALL = NO -EXTRACT_PRIVATE = NO -EXTRACT_STATIC = NO -EXTRACT_LOCAL_CLASSES = YES -EXTRACT_LOCAL_METHODS = NO -HIDE_UNDOC_MEMBERS = NO -HIDE_UNDOC_CLASSES = NO -HIDE_FRIEND_COMPOUNDS = NO -HIDE_IN_BODY_DOCS = NO -INTERNAL_DOCS = NO -CASE_SENSE_NAMES = YES -HIDE_SCOPE_NAMES = NO -SHOW_INCLUDE_FILES = YES -INLINE_INFO = YES -SORT_MEMBER_DOCS = YES -SORT_BRIEF_DOCS = NO -SORT_BY_SCOPE_NAME = NO -GENERATE_TODOLIST = YES -GENERATE_TESTLIST = YES -GENERATE_BUGLIST = YES -GENERATE_DEPRECATEDLIST= YES -ENABLED_SECTIONS = -MAX_INITIALIZER_LINES = 30 -SHOW_USED_FILES = YES -SHOW_DIRECTORIES = NO -FILE_VERSION_FILTER = -#--------------------------------------------------------------------------- -# configuration options related to warning and progress messages -#--------------------------------------------------------------------------- -QUIET = NO -WARNINGS = YES -WARN_IF_UNDOCUMENTED = YES -WARN_IF_DOC_ERROR = YES -WARN_NO_PARAMDOC = NO -WARN_FORMAT = "$file:$line: $text" -WARN_LOGFILE = -#--------------------------------------------------------------------------- -# configuration options related to the input files -#--------------------------------------------------------------------------- -INPUT = /home/kai/sourceforge/qmapcontrol/QMapControl/src -FILE_PATTERNS = *.c \ - *.cc \ - *.cxx \ - *.cpp \ - *.c++ \ - *.d \ - *.java \ - *.ii \ - *.ixx \ - *.ipp \ - *.i++ \ - *.inl \ - *.h \ - *.hh \ - *.hxx \ - *.hpp \ - *.h++ \ - *.idl \ - *.odl \ - *.cs \ - *.php \ - *.php3 \ - *.inc \ - *.m \ - *.mm \ - *.dox \ - *.py \ - *.C \ - *.CC \ - *.C++ \ - *.II \ - *.I++ \ - *.H \ - *.HH \ - *.H++ \ - *.CS \ - *.PHP \ - *.PHP3 \ - *.M \ - *.MM \ - *.PY \ - *.C \ - *.H \ - *.tlh \ - *.diff \ - *.patch \ - *.moc \ - *.xpm \ - *.dox -RECURSIVE = yes -EXCLUDE = -EXCLUDE_SYMLINKS = NO -EXCLUDE_PATTERNS = -EXAMPLE_PATH = -EXAMPLE_PATTERNS = * -EXAMPLE_RECURSIVE = NO -IMAGE_PATH = -INPUT_FILTER = -FILTER_PATTERNS = -FILTER_SOURCE_FILES = NO -#--------------------------------------------------------------------------- -# configuration options related to source browsing -#--------------------------------------------------------------------------- -SOURCE_BROWSER = NO -INLINE_SOURCES = NO -STRIP_CODE_COMMENTS = YES -REFERENCED_BY_RELATION = YES -REFERENCES_RELATION = YES -REFERENCES_LINK_SOURCE = YES -USE_HTAGS = NO -VERBATIM_HEADERS = YES -#--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- -ALPHABETICAL_INDEX = NO -COLS_IN_ALPHA_INDEX = 5 -IGNORE_PREFIX = -#--------------------------------------------------------------------------- -# configuration options related to the HTML output -#--------------------------------------------------------------------------- -GENERATE_HTML = YES -HTML_OUTPUT = html -HTML_FILE_EXTENSION = .html -HTML_HEADER = -HTML_FOOTER = -HTML_STYLESHEET = -HTML_ALIGN_MEMBERS = YES -GENERATE_HTMLHELP = NO -CHM_FILE = -HHC_LOCATION = -GENERATE_CHI = NO -BINARY_TOC = NO -TOC_EXPAND = NO -DISABLE_INDEX = NO -ENUM_VALUES_PER_LINE = 4 -GENERATE_TREEVIEW = NO -TREEVIEW_WIDTH = 250 -#--------------------------------------------------------------------------- -# configuration options related to the LaTeX output -#--------------------------------------------------------------------------- -GENERATE_LATEX = YES -LATEX_OUTPUT = latex -LATEX_CMD_NAME = latex -MAKEINDEX_CMD_NAME = makeindex -COMPACT_LATEX = NO -PAPER_TYPE = a4wide -EXTRA_PACKAGES = -LATEX_HEADER = -PDF_HYPERLINKS = NO -USE_PDFLATEX = NO -LATEX_BATCHMODE = NO -LATEX_HIDE_INDICES = NO -#--------------------------------------------------------------------------- -# configuration options related to the RTF output -#--------------------------------------------------------------------------- -GENERATE_RTF = NO -RTF_OUTPUT = rtf -COMPACT_RTF = NO -RTF_HYPERLINKS = NO -RTF_STYLESHEET_FILE = -RTF_EXTENSIONS_FILE = -#--------------------------------------------------------------------------- -# configuration options related to the man page output -#--------------------------------------------------------------------------- -GENERATE_MAN = NO -MAN_OUTPUT = man -MAN_EXTENSION = .3 -MAN_LINKS = NO -#--------------------------------------------------------------------------- -# configuration options related to the XML output -#--------------------------------------------------------------------------- -GENERATE_XML = yes -XML_OUTPUT = xml -XML_SCHEMA = -XML_DTD = -XML_PROGRAMLISTING = YES -#--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output -#--------------------------------------------------------------------------- -GENERATE_AUTOGEN_DEF = NO -#--------------------------------------------------------------------------- -# configuration options related to the Perl module output -#--------------------------------------------------------------------------- -GENERATE_PERLMOD = NO -PERLMOD_LATEX = NO -PERLMOD_PRETTY = YES -PERLMOD_MAKEVAR_PREFIX = -#--------------------------------------------------------------------------- -# Configuration options related to the preprocessor -#--------------------------------------------------------------------------- -ENABLE_PREPROCESSING = YES -MACRO_EXPANSION = NO -EXPAND_ONLY_PREDEF = NO -SEARCH_INCLUDES = YES -INCLUDE_PATH = -INCLUDE_FILE_PATTERNS = -PREDEFINED = -EXPAND_AS_DEFINED = -SKIP_FUNCTION_MACROS = YES -#--------------------------------------------------------------------------- -# Configuration::additions related to external references -#--------------------------------------------------------------------------- -TAGFILES = -GENERATE_TAGFILE = QMapControl.tag -ALLEXTERNALS = NO -EXTERNAL_GROUPS = YES -PERL_PATH = /usr/bin/perl -#--------------------------------------------------------------------------- -# Configuration options related to the dot tool -#--------------------------------------------------------------------------- -CLASS_DIAGRAMS = YES -HIDE_UNDOC_RELATIONS = YES -HAVE_DOT = NO -CLASS_GRAPH = YES -COLLABORATION_GRAPH = YES -GROUP_GRAPHS = YES -UML_LOOK = NO -TEMPLATE_RELATIONS = NO -INCLUDE_GRAPH = YES -INCLUDED_BY_GRAPH = YES -CALL_GRAPH = NO -CALLER_GRAPH = NO -GRAPHICAL_HIERARCHY = YES -DIRECTORY_GRAPH = YES -DOT_IMAGE_FORMAT = png -DOT_PATH = -DOTFILE_DIRS = -MAX_DOT_GRAPH_WIDTH = 1024 -MAX_DOT_GRAPH_HEIGHT = 1024 -MAX_DOT_GRAPH_DEPTH = 1000 -DOT_TRANSPARENT = NO -DOT_MULTI_TARGETS = NO -GENERATE_LEGEND = YES -DOT_CLEANUP = YES -#--------------------------------------------------------------------------- -# Configuration::additions related to the search engine -#--------------------------------------------------------------------------- -SEARCHENGINE = NO diff --git a/src/lib/qmapcontrol/src/circlepoint.cpp b/src/lib/qmapcontrol/src/circlepoint.cpp deleted file mode 100644 index 01945ba1bfd8c8c2b320c3c1b94869a935a70157..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/circlepoint.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "circlepoint.h" -namespace qmapcontrol -{ - CirclePoint::CirclePoint(qreal x, qreal y, int radius, QString name, Alignment alignment, QPen* pen) - : Point(x, y, name, alignment) - { - size = QSize(radius, radius); - mypixmap = new QPixmap(radius+1, radius+1); - mypixmap->fill(Qt::transparent); - QPainter painter(mypixmap); - if (pen != 0) - { - painter.setPen(*pen); - } - painter.drawEllipse(0,0,radius, radius); - } - - CirclePoint::CirclePoint(qreal x, qreal y, QString name, Alignment alignment, QPen* pen) - : Point(x, y, name, alignment) - { - int radius = 10; - size = QSize(radius, radius); - mypixmap = new QPixmap(radius+1, radius+1); - mypixmap->fill(Qt::transparent); - QPainter painter(mypixmap); - if (pen != 0) - { - painter.setPen(*pen); - } - painter.drawEllipse(0,0,radius, radius); - } - - CirclePoint::~CirclePoint() - { - delete mypixmap; - } - - void CirclePoint::setPen(QPen* pen) - { - mypen = pen; - mypixmap = new QPixmap(size.width()+1, size.height()+1); - mypixmap->fill(Qt::transparent); - QPainter painter(mypixmap); - painter.setPen(*pen); - painter.drawEllipse(0,0, size.width(), size.height()); - } -} diff --git a/src/lib/qmapcontrol/src/circlepoint.h b/src/lib/qmapcontrol/src/circlepoint.h deleted file mode 100644 index e6902f164e2eb04b6d2b1f6c99efc34cec6e56fd..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/circlepoint.h +++ /dev/null @@ -1,77 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef CIRCLEPOINT_H -#define CIRCLEPOINT_H - -#include "point.h" - -namespace qmapcontrol -{ - //! Draws a circle into the map - /*! This is a conveniece class for Point. - * It configures the pixmap of a Point to draw a circle. - * A QPen could be used to change the color or line-width of the circle - * - * @author Kai Winter - */ - class CirclePoint : public Point - { - public: - //! - /*! - * - * @param x longitude - * @param y latitude - * @param name name of the circle point - * @param alignment alignment (Middle or TopLeft) - * @param pen QPen for drawing - */ - CirclePoint(qreal x, qreal y, 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 - */ - CirclePoint(qreal x, qreal y, int radius = 10, QString name = QString(), Alignment alignment = Middle, QPen* pen=0); - virtual ~CirclePoint(); - - //! sets the QPen which is used for drawing the circle - /*! - * A QPen can be used to modify the look of the drawn circle - * @param pen the QPen which should be used for drawing - * @see http://doc.trolltech.com/4.3/qpen.html - */ - virtual void setPen(QPen* pen); - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/curve.cpp b/src/lib/qmapcontrol/src/curve.cpp deleted file mode 100644 index 4bb59eb77750e3459927d9d5eb2e8cac0c27db11..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/curve.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "curve.h" -namespace qmapcontrol -{ - Curve::Curve(QString name) - : Geometry(name) - { - } - - - Curve::~Curve() - { - } -} -// Geometry Curve::Clone(){} - -// QRectF Curve::GetBoundingBox(){} diff --git a/src/lib/qmapcontrol/src/curve.h b/src/lib/qmapcontrol/src/curve.h deleted file mode 100644 index 48f7e69aa4cd57aac1579a13d8d42deb2a5ce159..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/curve.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef CURVE_H -#define CURVE_H - -#include "geometry.h" -#include "point.h" - -namespace qmapcontrol -{ - //! A Curve Geometry, implemented to fullfil OGC Spec - /*! - * The Curve class is used by LineString as parent class. - * This class could not be used directly. - * - * From the OGC Candidate Implementation Specification: - * "A Curve is a 1-dimensional geometric object usually stored as a sequence of Points, with the subtype of Curve - * specifying the form of the interpolation between Points. This specification defines only one subclass of Curve, - * LineString, which uses a linear interpolation between Points." - * @author Kai Winter - */ - class Curve : public Geometry - { - Q_OBJECT - public: - virtual ~Curve(); - - double Length; - -// virtual Geometry Clone(); -// virtual QRectF GetBoundingBox(); - -// virtual Point EndPoint() = 0; -// virtual Point StartPoint() = 0; -// virtual Point Value() = 0; - - protected: - Curve(QString name = QString()); - virtual void draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &screensize, const QPoint offset) = 0; - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/geometry.cpp b/src/lib/qmapcontrol/src/geometry.cpp deleted file mode 100644 index 936d89120b49938839e23a20517c02e1df571f4e..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/geometry.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "geometry.h" -namespace qmapcontrol -{ - Geometry::Geometry(QString name) - : GeometryType("Geometry"), myparentGeometry(0), mypen(0), visible(true), myname(name) - { - } - - - Geometry::~Geometry() - { - } - - QString Geometry::name() const - { - return myname; - } - Geometry* Geometry::parentGeometry() const - { - return myparentGeometry; - } - void Geometry::setParentGeometry(Geometry* geom) - { - myparentGeometry = geom; - } - bool Geometry::hasPoints() const - { - return false; - } - bool Geometry::hasClickedPoints() const - { - return false; - } - QList Geometry::clickedPoints() - { - QList tmp; - return tmp; - } - - bool Geometry::isVisible() const - { - return visible; - } - void Geometry::setVisible(bool visible) - { - this->visible = visible; - emit(updateRequest(boundingBox())); - } - - void Geometry::setName(QString name) - { - myname = name; - } - - void Geometry::setPen(QPen* pen) - { - mypen = pen; - } - QPen* Geometry::pen() const - { - return mypen; - } -} diff --git a/src/lib/qmapcontrol/src/geometry.h b/src/lib/qmapcontrol/src/geometry.h deleted file mode 100644 index 6c94f01384b7b4dad32a61686d1539390b6727a3..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/geometry.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef GEOMETRY_H -#define GEOMETRY_H -#include -#include -#include -#include "mapadapter.h" - -namespace qmapcontrol -{ - class Point; - //! Main class for objects that should be painted in maps - /*! - * Geometry is the root class of the hierarchy. Geometry is an abstract (non-instantiable) class. - * - * This class and the derived classes Point, Curve and LineString are leant on the Simple - * Feature Specification of the Open Geospatial Consortium. - * @see www.opengeospatial.com - * - * @author Kai Winter - */ - class Geometry : public QObject - { - friend class LineString; - Q_OBJECT - public: - explicit Geometry(QString name = QString()); - virtual ~Geometry(); - - QString GeometryType; - - //! - /*! returns true if the given Geometry is equal to this Geometry - * not implemented yet! - * @param geom The Geometry to be tested - * @return true if the given Geometry is equal to this - */ - bool Equals(Geometry* geom); - - //! returns a String representation of this Geometry - /*! - * not implemented yet! - * @return a String representation of this Geometry - */ - QString toString(); - - //! returns the name of this Geometry - /*! - * @return the name of this Geometry - */ - QString name() const; - - //! returns the parent Geometry of this Geometry - /*! - * A LineString is a composition of many Points. This methods returns the parent (the LineString) of a Point - * @return the parent Geometry of this Geometry - */ - Geometry* parentGeometry() const; - - //! returns true if this Geometry is visible - /*! - * @return true if this Geometry is visible - */ - bool isVisible() const; - - //! sets the name of the geometry - /*! - * @param name the new name of the geometry - */ - void setName(QString name); - - //! returns the QPen which is used on drawing - /*! - * The pen is set depending on the Geometry. A CirclePoint for example takes one with the constructor. - * @return the QPen which is used for drawing - */ - QPen* pen() const; - - //! returns the BoundingBox - /*! - * The bounding box in world coordinates - * @return the BoundingBox - */ - virtual QRectF boundingBox()=0; - virtual bool Touches(Point* geom, const MapAdapter* mapadapter)=0; - virtual void draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &viewport, const QPoint offset)=0; - virtual bool hasPoints() const; - virtual bool hasClickedPoints() const; - virtual void setPen(QPen* pen); - virtual QList clickedPoints(); - virtual QList points()=0; - - private: - Geometry* myparentGeometry; - Geometry(const Geometry& old); - Geometry& operator=(const Geometry& rhs); - - protected: - QPen* mypen; - bool visible; - QString myname; - void setParentGeometry(Geometry* geom); - - - signals: - void updateRequest(Geometry* geom); - void updateRequest(QRectF rect); - //! This signal is emitted when a Geometry is clicked - /*! - * A Geometry is clickable, if the containing layer is clickable. - * The objects emits a signal if it gets clicked - * @param geometry The clicked Geometry - * @param point -unused- - */ - void geometryClicked(Geometry* geometry, QPoint point); - - //! A Geometry emits this signal, when its position gets changed - /*! - * @param geom the Geometry - */ - void positionChanged(Geometry* geom); - - public slots: - //! if visible is true, the layer is made visible - /*! - * @param visible if the layer should be visible - */ - virtual void setVisible(bool visible); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/geometrylayer.cpp b/src/lib/qmapcontrol/src/geometrylayer.cpp deleted file mode 100644 index e24506add3767ef2c233b3e40ea859a1505e2f2c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/geometrylayer.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "geometrylayer.h" -namespace qmapcontrol -{ - GeometryLayer::GeometryLayer(QString layername, MapAdapter* mapadapter, bool takeevents) - : Layer(layername, mapadapter, Layer::GeometryLayer, takeevents) - { - } - - - GeometryLayer::~GeometryLayer() - { - } -} diff --git a/src/lib/qmapcontrol/src/geometrylayer.h b/src/lib/qmapcontrol/src/geometrylayer.h deleted file mode 100644 index 01b330ebae3c91f90a8fa09f8633fff63f57a77c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/geometrylayer.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef GEOMETRYLAYER_H -#define GEOMETRYLAYER_H - -#include "layer.h" - -namespace qmapcontrol -{ - //! GeometryLayer class - /*! - * There are two different layer types: - * - MapLayer: Displays Maps, but also Geometries. The configuration for displaying maps have to be done in the MapAdapter - * - GeometryLayer: Only displays Geometry objects. - * - * MapLayers also can display Geometry objects. The difference to the GeometryLayer is the repainting. Objects that are - * added to a MapLayer are "baken" on the map. This means, when you change it´s position for example the changes are - * not visible until a new offscreen image has been drawn. If you have "static" Geometries which won´t change their - * position this is fine. But if you want to change the objects position or pen you should use a GeometryLayer. Those - * are repainted immediately on changes. - * - * @author Kai Winter - */ - class GeometryLayer : public Layer - { - Q_OBJECT - public: - //! GeometryLayer constructor - /*! - * This is used to construct a map layer. - * - * @param layername The name of the Layer - * @param mapadapter The MapAdapter which does coordinate translation and Query-String-Forming - * @param takeevents Should the Layer receive MouseEvents? This is set to true by default. Setting it to false could - * be something like a "speed up hint" - */ - GeometryLayer(QString layername, MapAdapter* mapadapter, bool takeevents=true); - virtual ~GeometryLayer(); - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/googlemapadapter.cpp b/src/lib/qmapcontrol/src/googlemapadapter.cpp deleted file mode 100644 index cfc2ca6cf303219fa1781bc85cce3002097d2172..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/googlemapadapter.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "googlemapadapter.h" -namespace qmapcontrol -{ - GoogleMapAdapter::GoogleMapAdapter() - : TileMapAdapter("mt2.google.com", "/mt?n=404&x=%2&y=%3&zoom=%1", 256, 17, 0) -// : TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17) - { - } - - GoogleMapAdapter::~GoogleMapAdapter() - { - } -} diff --git a/src/lib/qmapcontrol/src/googlemapadapter.h b/src/lib/qmapcontrol/src/googlemapadapter.h deleted file mode 100644 index cf5e38e91f085d82a66898e2f73bdae57fc98573..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/googlemapadapter.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef GOOGLEMAPADAPTER_H -#define GOOGLEMAPADAPTER_H - -#include "tilemapadapter.h" - -namespace qmapcontrol -{ - //! MapAdapter for Google - /*! - * This is a conveniece class, which extends and configures a TileMapAdapter - * @author Kai Winter - */ - class GoogleMapAdapter : public TileMapAdapter - { - Q_OBJECT - public: - //! constructor - /*! - * This construct a Google Adapter - */ - GoogleMapAdapter(); - virtual ~GoogleMapAdapter(); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/googlesatmapadapter.cpp b/src/lib/qmapcontrol/src/googlesatmapadapter.cpp deleted file mode 100755 index cfa54ea47c7135703317da7af11ffe1486be88ba..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/googlesatmapadapter.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "googlesatmapadapter.h" - -#include -namespace qmapcontrol -{ -GoogleSatMapAdapter::GoogleSatMapAdapter() - : MapAdapter("kh2.google.com", "/kh?n=404&v=8&t=trtqtt", 256, 0, 19) -{ -// name = "googlesat"; - - numberOfTiles = pow(2, current_zoom+0.0); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; -} - -GoogleSatMapAdapter::~GoogleSatMapAdapter() -{ -} - -QString GoogleSatMapAdapter::getHost() const -{ - int random = qrand() % 4; - return QString("kh%1.google.com").arg(random); -} - -QPoint GoogleSatMapAdapter::coordinateToDisplay(const QPointF& coordinate) const -{ -// double x = ((coordinate.x()+180)*(tilesize*numberOfTiles/360)); -// double y = (((coordinate.y()*-1)+90)*(tilesize*numberOfTiles/180)); - - qreal x = (coordinate.x()+180.) * (numberOfTiles*mytilesize)/360.; // coord to pixel! -// double y = -1*(coordinate.y()-90) * (numberOfTiles*tilesize)/180.; // coord to pixel! - qreal y = (getMercatorYCoord(coordinate.y())-M_PI) * -1 * (numberOfTiles*mytilesize)/(2*M_PI); // coord to pixel! - return QPoint(int(x), int(y)); -} - -QPointF GoogleSatMapAdapter::displayToCoordinate(const QPoint& point) const -{ -// double lon = ((point.x()/tilesize*numberOfTiles)*360)-180; -// double lat = (((point.y()/tilesize*numberOfTiles)*180)-90)*-1; - - qreal lon = (point.x()*(360./(numberOfTiles*mytilesize)))-180.; -// double lat = -(point.y()*(180./(numberOfTiles*tilesize)))+90; -// qreal lat = getMercatorLatitude(point.y()*-1*(2*M_PI/(numberOfTiles*tilesize)) + M_PI); - qreal lat = lat *180./M_PI; - return QPointF(lon, lat); -} - -qreal GoogleSatMapAdapter::getMercatorLatitude(qreal YCoord) const -{ - // http://welcome.warnercnr.colostate.edu/class_info/nr502/lg4/projection_mathematics/converting.html - if (YCoord > M_PI) return 9999.; - if (YCoord < -M_PI) return -9999.; - - qreal t = atan(exp(YCoord)); - qreal res = (2.*(t))-(M_PI/2.); - return res; -} - -qreal GoogleSatMapAdapter::getMercatorYCoord(qreal lati) const -{ - qreal lat = lati; - - // conversion degre=>radians - qreal phi = M_PI * lat / 180; - - qreal res; - //double temp = Math.Tan(Math.PI / 4 - phi / 2); - //res = Math.Log(temp); - res = 0.5 * log((1 + sin(phi)) / (1 - sin(phi))); - - return res; -} - -void GoogleSatMapAdapter::zoom_in() -{ - current_zoom+=1; - numberOfTiles = pow(2, current_zoom+0.0); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; -} - -void GoogleSatMapAdapter::zoom_out() -{ - current_zoom-=1; - numberOfTiles = pow(2, current_zoom+0.0); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; -} - -bool GoogleSatMapAdapter::isValid(int x, int y, int z) const -{ - if ((x>=0 && x < numberOfTiles) && (y>=0 && y < numberOfTiles) && z>=0) - { - return true; - } - return false; -} -QString GoogleSatMapAdapter::query(int i, int j, int z) const -{ - return getQ(-180+i*coord_per_x_tile, - 90-(j+1)*coord_per_y_tile, z); -} - -QString GoogleSatMapAdapter::getQ(qreal longitude, qreal latitude, int zoom) const -{ - qreal xmin=-180; - qreal xmax=180; - qreal ymin=-90; - qreal ymax=90; - - qreal xmoy=0; - qreal ymoy=0; - QString location="t"; - - //Google uses a latitude divided by 2; - qreal halflat = latitude; - - for (int i = 0; i < zoom; i++) - { - xmoy = (xmax + xmin) / 2; - ymoy = (ymax + ymin) / 2; - if (halflat >= ymoy) //upper part (q or r) - { - ymin = ymoy; - if (longitude < xmoy) - { /*q*/ - location+= "q"; - xmax = xmoy; - } - else - {/*r*/ - location+= "r"; - xmin = xmoy; - } - } - else //lower part (t or s) - { - ymax = ymoy; - if (longitude < xmoy) - { /*t*/ - - location+= "t"; - xmax = xmoy; - } - else - {/*s*/ - location+= "s"; - xmin = xmoy; - } - } - } - return QString("/kh?n=404&v=24&t=%1").arg(location); -} -} - diff --git a/src/lib/qmapcontrol/src/googlesatmapadapter.h b/src/lib/qmapcontrol/src/googlesatmapadapter.h deleted file mode 100755 index 8293b15a968fc48cfde7fe8e252a7a9f97f905d6..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/googlesatmapadapter.h +++ /dev/null @@ -1,74 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef GOOGLESATMAPADAPTER_H -#define GOOGLESATMAPADAPTER_H - -#include "mapadapter.h" -namespace qmapcontrol -{ -//! MapAdapter for Google -/*! - * This is a conveniece class, which extends and configures a TileMapAdapter - * @author Kai Winter -*/ -class GoogleSatMapAdapter : public MapAdapter -{ - Q_OBJECT - public: - //! constructor - /*! - * This construct a Google Adapter - */ - GoogleSatMapAdapter(); - virtual ~GoogleSatMapAdapter(); - - virtual QPoint coordinateToDisplay(const QPointF&) const; - virtual QPointF displayToCoordinate(const QPoint&) const; - - //! returns the host of this MapAdapter - /*! - * @return the host of this MapAdapter - */ - QString getHost () const; - - - protected: - virtual void zoom_in(); - virtual void zoom_out(); - virtual QString query(int x, int y, int z) const; - virtual bool isValid(int x, int y, int z) const; - - private: - virtual QString getQ(qreal longitude, qreal latitude, int zoom) const; - qreal getMercatorLatitude(qreal YCoord) const; - qreal getMercatorYCoord(qreal lati) const; - - qreal coord_per_x_tile; - qreal coord_per_y_tile; - int srvNum; -}; -} -#endif diff --git a/src/lib/qmapcontrol/src/gps_position.cpp b/src/lib/qmapcontrol/src/gps_position.cpp deleted file mode 100644 index fc8f25457be74e3e69abe8b6c469209e198af5c2..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/gps_position.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "gps_position.h" -namespace qmapcontrol -{ - GPS_Position::GPS_Position(float time, float longitude, QString longitude_dir, float latitude, QString latitude_dir) - :time(time), longitude(longitude), latitude(latitude), longitude_dir(longitude_dir), latitude_dir(latitude_dir) - { - } -} diff --git a/src/lib/qmapcontrol/src/gps_position.h b/src/lib/qmapcontrol/src/gps_position.h deleted file mode 100644 index ba61caa3537291b030ed7a2a410ad45e79a990af..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/gps_position.h +++ /dev/null @@ -1,53 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef GPS_POSITION_H -#define GPS_POSITION_H - -#include - -namespace qmapcontrol -{ - //! Represents a coordinate from a GPS receiver - /*! - * This class is used to represent a coordinate which has been parsed from a NMEA string. - * This is not fully integrated in the API. An example which uses this data type can be found under Samples. - * @author Kai Winter - */ - class GPS_Position - { - public: - GPS_Position(float time, float longitude, QString longitude_dir, float latitude, QString latitude_dir); - float time; /*!< time of the string*/ - float longitude; /*!< longitude coordinate*/ - float latitude; /*!< latitude coordinate*/ - - private: - QString longitude_dir; - QString latitude_dir; - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/imagemanager.cpp b/src/lib/qmapcontrol/src/imagemanager.cpp deleted file mode 100644 index da8f115a4e70872b65e3c6be81242eb76f1b0e8e..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/imagemanager.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "imagemanager.h" -namespace qmapcontrol -{ - ImageManager* ImageManager::m_Instance = 0; - ImageManager::ImageManager(QObject* parent) - :QObject(parent), emptyPixmap(QPixmap(1,1)), net(new MapNetwork(this)), doPersistentCaching(false) - { - emptyPixmap.fill(Qt::transparent); - - if (QPixmapCache::cacheLimit() <= 20000) - { - QPixmapCache::setCacheLimit(20000); // in kb - } - } - - - ImageManager::~ImageManager() - { - delete net; - } - - QPixmap ImageManager::getImage(const QString& host, const QString& url) - { -// qDebug() << "ImageManager::getImage"; - QPixmap pm; -// pm.fill(Qt::black); - - // is image cached (memory) or currently loading? - if (!QPixmapCache::find(url, pm) && !net->imageIsLoading(url)) -// if (!images.contains(url) && !net->imageIsLoading(url)) - { - // image cached (persistent)? - if (doPersistentCaching && tileExist(url)) - { - loadTile(url,pm); - QPixmapCache::insert(url.toAscii().toBase64(), pm); - } - else - { - // load from net, add empty image - net->loadImage(host, url); - //QPixmapCache::insert(url, emptyPixmap); - return emptyPixmap; - } - } - return pm; - } - - QPixmap ImageManager::prefetchImage(const QString& host, const QString& url) - { -#ifdef Q_WS_QWS - // on mobile devices we don´t want the display resfreshing when tiles are received which are - // prefetched... This is a performance issue, because mobile devices are very slow in - // repainting the screen - prefetch.append(url); -#endif - return getImage(host, url); - } - - void ImageManager::receivedImage(const QPixmap pixmap, const QString& url) - { -// qDebug() << "ImageManager::receivedImage"; - QPixmapCache::insert(url, pixmap); -// images[url] = pixmap; - - // needed? - if (doPersistentCaching && !tileExist(url) ) - saveTile(url,pixmap); - -// ((Layer*)this->parent())->imageReceived(); - - if (!prefetch.contains(url)) - { - emit(imageReceived()); - } - else - { - -#ifdef Q_WS_QWS - prefetch.remove(prefetch.indexOf(url)); -#endif - } - } - - void ImageManager::loadingQueueEmpty() - { - emit(loadingFinished()); -// ((Layer*)this->parent())->removeZoomImage(); -// qDebug() << "size of image-map: " << images.size(); -// qDebug() << "size: " << QPixmapCache::cacheLimit(); - } - - void ImageManager::abortLoading() - { - net->abortLoading(); - } - void ImageManager::setProxy(QString host, int port) - { - net->setProxy(host, port); - } - - void ImageManager::setCacheDir(const QDir& path) - { - doPersistentCaching = true; - cacheDir = path; - if (!cacheDir.exists()) - { - cacheDir.mkpath(cacheDir.absolutePath()); - } - } - - bool ImageManager::saveTile(QString tileName,QPixmap tileData) - { - tileName.replace("/","-"); - - QFile file(cacheDir.absolutePath() + "/" + tileName.toAscii().toBase64()); - - qDebug() << "writing: " << file.fileName(); - if (!file.open(QIODevice::ReadWrite )){ - qDebug()<<"error reading file"; - return false; - } - QByteArray bytes; - QBuffer buffer(&bytes); - buffer.open(QIODevice::WriteOnly); - tileData.save(&buffer, "PNG"); - - file.write(bytes); - file.close(); - return true; - } - bool ImageManager::loadTile(QString tileName,QPixmap &tileData) - { - tileName.replace("/","-"); - QFile file(cacheDir.absolutePath() + "/" + tileName.toAscii().toBase64()); - if (!file.open(QIODevice::ReadOnly )) { - return false; - } - tileData.loadFromData( file.readAll() ); - - file.close(); - return true; - } - bool ImageManager::tileExist(QString tileName) - { - tileName.replace("/","-"); - QFile file(cacheDir.absolutePath() + "/" + tileName.toAscii().toBase64()); - if (file.exists()) - return true; - else - return false; - } -} diff --git a/src/lib/qmapcontrol/src/imagemanager.h b/src/lib/qmapcontrol/src/imagemanager.h deleted file mode 100644 index 04de8f9769b5ca1d8719282115a97e6d36d3e34c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/imagemanager.h +++ /dev/null @@ -1,124 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef IMAGEMANAGER_H -#define IMAGEMANAGER_H - -#include -#include -#include -#include -#include -#include -#include -#include "mapnetwork.h" - -namespace qmapcontrol -{ - class MapNetwork; - /** - @author Kai Winter - */ - class ImageManager : public QObject - { - Q_OBJECT; - public: - static ImageManager* instance() - { - if(!m_Instance) - { - m_Instance = new ImageManager; - } - return m_Instance; - } - - ~ImageManager(); - - //! returns a QPixmap of the asked image - /*! - * If this component doesn´t have the image a network query gets started to load it. - * @param host the host of the image - * @param path the path to the image - * @return the pixmap of the asked image - */ - QPixmap getImage(const QString& host, const QString& path); - - QPixmap prefetchImage(const QString& host, const QString& path); - - void receivedImage(const QPixmap pixmap, const QString& url); - - /*! - * This method is called by MapNetwork, after all images in its queue were loaded. - * The ImageManager emits a signal, which is used in MapControl to remove the zoom image. - * The zoom image should be removed on Tile Images with transparency. - * Else the zoom image stay visible behind the newly loaded tiles. - */ - void loadingQueueEmpty(); - - /*! - * Aborts all current loading threads. - * This is useful when changing the zoom-factor, though newly needed images loads faster - */ - void abortLoading(); - - //! sets the proxy for HTTP connections - /*! - * This method sets the proxy for HTTP connections. - * This is not provided by the current Qtopia version! - * @param host the proxy´s hostname or ip - * @param port the proxy´s port - */ - void setProxy(QString host, int port); - - //! sets the cache directory for persistently saving map tiles - /*! - * - * @param path the path where map tiles should be stored - * @todo add maximum size - */ - void setCacheDir(const QDir& path); - - private: - ImageManager(QObject* parent = 0); - ImageManager(const ImageManager&); - ImageManager& operator=(const ImageManager&); - QPixmap emptyPixmap; - MapNetwork* net; - QVector prefetch; - QDir cacheDir; - bool doPersistentCaching; - - static ImageManager* m_Instance; - - bool saveTile(QString tileName,QPixmap tileData); - bool loadTile(QString tileName,QPixmap &tileData); - bool tileExist(QString tileName); - - signals: - void imageReceived(); - void loadingFinished(); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/imagepoint.cpp b/src/lib/qmapcontrol/src/imagepoint.cpp deleted file mode 100644 index 1b9bef12c3f3b9ac3e24435df683823af7250200..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/imagepoint.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "imagepoint.h" -namespace qmapcontrol -{ - ImagePoint::ImagePoint(qreal x, qreal y, QString filename, QString name, Alignment alignment) - : Point(x, y, name, alignment) - { -// qDebug() << "loading image: " << filename; - mypixmap = new QPixmap(filename); - size = mypixmap->size(); -// qDebug() << "image size: " << size; - } - - ImagePoint::ImagePoint(qreal x, qreal y, QPixmap* pixmap, QString name, Alignment alignment) - : Point(x, y, name, alignment) - { - mypixmap = pixmap; - size = mypixmap->size(); - } - - - ImagePoint::~ImagePoint() - { - } -} diff --git a/src/lib/qmapcontrol/src/imagepoint.h b/src/lib/qmapcontrol/src/imagepoint.h deleted file mode 100644 index 682937024c12e61c25e48ca2c8648fdde776bd87..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/imagepoint.h +++ /dev/null @@ -1,71 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef IMAGEPOINT_H -#define IMAGEPOINT_H - -#include "point.h" - -namespace qmapcontrol -{ - - //! Draws an image into the map - /*! This is a convenience class for Point. - * It configures the pixmap of a Point to draw the given image. - * The image will be loaded from the given path and written in the points pixmap. - * - * @author Kai Winter - */ - class ImagePoint : public Point - { - public: - //! Creates a point which loads and displays the given image file - /*! - * Use this contructor to load the given image file and let the point display it. - * When you want multiple points to display the same image, use the other contructor and pass a pointer to that image. - * @param x longitude - * @param y latitude - * @param filename the file which should be loaded and displayed - * @param name the name of the image point - * @param alignment alignment (Middle or TopLeft) - */ - ImagePoint(qreal x, qreal y, QString filename, QString name = QString(), Alignment alignment = Middle); - - //! Creates a point which displays the given image - /*! - * Use this contructor to display the given image. - * You have to load that image yourself, but can use it for multiple points. - * @param x longitude - * @param y latitude - * @param pixmap pointer to the image pixmap - * @param name the name of the image point - * @param alignment alignment (Middle or TopLeft) - */ - ImagePoint(qreal x, qreal y, QPixmap* pixmap, QString name = QString(), Alignment alignment = Middle); - virtual ~ImagePoint(); - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/images/marker1.png b/src/lib/qmapcontrol/src/images/marker1.png deleted file mode 100644 index 60b2f9e3c200cf817c78a7bef11ad51598930b91..0000000000000000000000000000000000000000 Binary files a/src/lib/qmapcontrol/src/images/marker1.png and /dev/null differ diff --git a/src/lib/qmapcontrol/src/images/marker2.png b/src/lib/qmapcontrol/src/images/marker2.png deleted file mode 100644 index 7ea3fa10f58d3987787cbd27c4372f71e1286883..0000000000000000000000000000000000000000 Binary files a/src/lib/qmapcontrol/src/images/marker2.png and /dev/null differ diff --git a/src/lib/qmapcontrol/src/images/marker3.png b/src/lib/qmapcontrol/src/images/marker3.png deleted file mode 100644 index 2aa2817652df9e6bd6133f34653b5bdbc8998e00..0000000000000000000000000000000000000000 Binary files a/src/lib/qmapcontrol/src/images/marker3.png and /dev/null differ diff --git a/src/lib/qmapcontrol/src/layer.cpp b/src/lib/qmapcontrol/src/layer.cpp deleted file mode 100644 index 62a9d99ffb1d35634630d5b81a9a898fa59a4646..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/layer.cpp +++ /dev/null @@ -1,295 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "layer.h" -namespace qmapcontrol -{ - Layer::Layer(QString layername, MapAdapter* mapadapter, enum LayerType layertype, bool takeevents) - :visible(true), mylayername(layername), mylayertype(layertype), mapAdapter(mapadapter), takeevents(takeevents), myoffscreenViewport(QRect(0,0,0,0)) - { -// qDebug() << "creating new Layer: " << layername << ", type: " << contents; -// qDebug() << this->layertype; - } - - Layer::~Layer() - { - delete mapAdapter; - } - - void Layer::setSize(QSize size) - { - this->size = size; - screenmiddle = QPoint(size.width()/2, size.height()/2); -// QMatrix mat; -// mat.translate(480/2, 640/2); -// mat.rotate(45); -// mat.translate(-480/2,-640/2); -// screenmiddle = mat.map(screenmiddle); - - } - - QString Layer::layername() const - { - return mylayername; - } - - const MapAdapter* Layer::mapadapter() const - { - return mapAdapter; - } - - void Layer::setVisible(bool visible) - { - this->visible = visible; - emit(updateRequest()); - } - - void Layer::addGeometry(Geometry* geom) - { -// qDebug() << geom->getName() << ", " << geom->getPoints().at(0)->getWidget(); - - geometries.append(geom); - emit(updateRequest(geom->boundingBox())); - // a geometry can request a redraw, e.g. when its position has been changed - connect(geom, SIGNAL(updateRequest(QRectF)), - this, SIGNAL(updateRequest(QRectF))); - } - void Layer::removeGeometry(Geometry* geometry) - { - for (int i=0; izoom_in(); - } - void Layer::zoomOut() const - { - mapAdapter->zoom_out(); - } - - void Layer::mouseEvent(const QMouseEvent* evnt, const QPoint mapmiddle_px) - { - if (takesMouseEvents()) - { - if (evnt->button() == Qt::LeftButton && evnt->type() == QEvent::MouseButtonPress) - { - // check for collision - QPointF c = mapAdapter->displayToCoordinate(QPoint(evnt->x()-screenmiddle.x()+mapmiddle_px.x(), - evnt->y()-screenmiddle.y()+mapmiddle_px.y())); - Point* tmppoint = new Point(c.x(), c.y()); - for (int i=0; iisVisible() && geometries.at(i)->Touches(tmppoint, mapAdapter)) - -// if (geometries.at(i)->Touches(c, mapAdapter)) - { - emit(geometryClicked(geometries.at(i), QPoint(evnt->x(), evnt->y()))); - } - } - delete tmppoint; - } - } - } - - bool Layer::takesMouseEvents() const - { - return takeevents; - } - - void Layer::drawYourImage(QPainter* painter, const QPoint mapmiddle_px) const - { - if (mylayertype == MapLayer) - { -// qDebug() << ":: " << mapmiddle_px; -// QMatrix mat; -// mat.translate(480/2, 640/2); -// mat.rotate(45); -// mat.translate(-480/2,-640/2); - -// mapmiddle_px = mat.map(mapmiddle_px); -// qDebug() << ":: " << mapmiddle_px; - _draw(painter, mapmiddle_px); - } - - drawYourGeometries(painter, QPoint(mapmiddle_px.x()-screenmiddle.x(), mapmiddle_px.y()-screenmiddle.y()), myoffscreenViewport); - } - void Layer::drawYourGeometries(QPainter* painter, const QPoint mapmiddle_px, QRect viewport) const - { - QPoint offset; - if (mylayertype == MapLayer) - offset = mapmiddle_px; - else - offset = mapmiddle_px-screenmiddle; - - painter->translate(-mapmiddle_px+screenmiddle); - for (int i=0; idraw(painter, mapAdapter, viewport, offset); - } - painter->translate(mapmiddle_px-screenmiddle); - - } - void Layer::_draw(QPainter* painter, const QPoint mapmiddle_px) const - { - // screen middle rotieren... - - int tilesize = mapAdapter->tilesize(); - int cross_x = int(mapmiddle_px.x())%tilesize; // position on middle tile - int cross_y = int(mapmiddle_px.y())%tilesize; -// qDebug() << screenmiddle << " - " << cross_x << ", " << cross_y; - - // calculate how many surrounding tiles have to be drawn to fill the display - int space_left = screenmiddle.x() - cross_x; - int tiles_left = space_left/tilesize; - if (space_left>0) - tiles_left+=1; - - int space_above = screenmiddle.y() - cross_y; - int tiles_above = space_above/tilesize; - if (space_above>0) - tiles_above+=1; - - int space_right = screenmiddle.x() - (tilesize-cross_x); - int tiles_right = space_right/tilesize; - if (space_right>0) - tiles_right+=1; - - int space_bottom = screenmiddle.y() - (tilesize-cross_y); - int tiles_bottom = space_bottom/tilesize; - if (space_bottom>0) - tiles_bottom+=1; - -// int tiles_displayed = 0; - int mapmiddle_tile_x = mapmiddle_px.x()/tilesize; - int mapmiddle_tile_y = mapmiddle_px.y()/tilesize; - - const QPoint from = QPoint((-tiles_left+mapmiddle_tile_x)*tilesize, (-tiles_above+mapmiddle_tile_y)*tilesize); - const QPoint to = QPoint((tiles_right+mapmiddle_tile_x+1)*tilesize, (tiles_bottom+mapmiddle_tile_y+1)*tilesize); - - myoffscreenViewport = QRect(from, to); - - if (mapAdapter->isValid(mapmiddle_tile_x, mapmiddle_tile_y, mapAdapter->currentZoom())) - { - painter->drawPixmap(-cross_x+size.width(), - -cross_y+size.height(), - ImageManager::instance()->getImage(mapAdapter->host(), mapAdapter->query(mapmiddle_tile_x, mapmiddle_tile_y, mapAdapter->currentZoom()))); - } - - for (int i=-tiles_left+mapmiddle_tile_x; i<=tiles_right+mapmiddle_tile_x; i++) - { - for (int j=-tiles_above+mapmiddle_tile_y; j<=tiles_bottom+mapmiddle_tile_y; j++) - { - // check if image is valid - if (!(i==mapmiddle_tile_x && j==mapmiddle_tile_y)) - if (mapAdapter->isValid(i, j, mapAdapter->currentZoom())) - { - - painter->drawPixmap(((i-mapmiddle_tile_x)*tilesize)-cross_x+size.width(), - ((j-mapmiddle_tile_y)*tilesize)-cross_y+size.height(), - ImageManager::instance()->getImage(mapAdapter->host(), mapAdapter->query(i, j, mapAdapter->currentZoom()))); -// if (QCoreApplication::hasPendingEvents()) -// QCoreApplication::processEvents(); - } - } - } - - - // PREFETCHING - int upper = mapmiddle_tile_y-tiles_above-1; - int right = mapmiddle_tile_x+tiles_right+1; - int left = mapmiddle_tile_x-tiles_right-1; - int lower = mapmiddle_tile_y+tiles_bottom+1; - - int j = upper; - for (int i=left; i<=right; i++) - { - if (mapAdapter->isValid(i, j, mapAdapter->currentZoom())) - ImageManager::instance()->prefetchImage(mapAdapter->host(), mapAdapter->query(i, j, mapAdapter->currentZoom())); - } - j = lower; - for (int i=left; i<=right; i++) - { - if (mapAdapter->isValid(i, j, mapAdapter->currentZoom())) - ImageManager::instance()->prefetchImage(mapAdapter->host(), mapAdapter->query(i, j, mapAdapter->currentZoom())); - } - int i = left; - for (int j=upper+1; j<=lower-1; j++) - { - if (mapAdapter->isValid(i, j, mapAdapter->currentZoom())) - ImageManager::instance()->prefetchImage(mapAdapter->host(), mapAdapter->query(i, j, mapAdapter->currentZoom())); - } - i = right; - for (int j=upper+1; j<=lower-1; j++) - { - if (mapAdapter->isValid(i, j, mapAdapter->currentZoom())) - ImageManager::instance()->prefetchImage(mapAdapter->host(), mapAdapter->query(i, j, mapAdapter->currentZoom())); - } - - } - - QRect Layer::offscreenViewport() const - { - return myoffscreenViewport; - } - - void Layer::moveWidgets(const QPoint mapmiddle_px) const - { - for (int i=0; iGeometryType == "Point") - { - if (((Point*)geom)->widget()!=0) - { - QPoint topleft_relative = QPoint(mapmiddle_px-screenmiddle); - ((Point*)geom)->drawWidget(mapAdapter, topleft_relative); - } - } - } - } - Layer::LayerType Layer::layertype() const - { - return mylayertype; - } - - void Layer::setMapAdapter(MapAdapter* mapadapter) - { - - mapAdapter = mapadapter; - } -} diff --git a/src/lib/qmapcontrol/src/layer.h b/src/lib/qmapcontrol/src/layer.h deleted file mode 100644 index 664a7fd06aa74ef83f9827697c796e695588d248..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/layer.h +++ /dev/null @@ -1,173 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef LAYER_H -#define LAYER_H - -#include -#include -#include -#include - -#include "mapadapter.h" -#include "layermanager.h" -#include "geometry.h" -#include "point.h" - -#include "wmsmapadapter.h" -#include "tilemapadapter.h" - -namespace qmapcontrol -{ - //! Layer class - /*! - * There are two different layer types: - * - MapLayer: Displays Maps, but also Geometries. The configuration for displaying maps have to be done in the MapAdapter - * - GeometryLayer: Only displays Geometry objects. - * - * MapLayers also can display Geometry objects. The difference to the GeometryLayer is the repainting. Objects that are - * added to a MapLayer are "baken" on the map. This means, when you change it´s position for example the changes are - * not visible until a new offscreen image has been drawn. If you have "static" Geometries which won´t change their - * position this is fine. But if you want to change the objects position or pen you should use a GeometryLayer. Those - * are repainted immediately on changes. - * You can either use this class and give a layertype on creation or you can use the classes MapLayer and GeometryLayer. - * - * @author Kai Winter - */ - class Layer : public QObject - { - Q_OBJECT - public: - friend class LayerManager; - - //! sets the type of a layer, see Layer class doc for further information - enum LayerType - { - MapLayer, /*!< uses the MapAdapter to display maps, only gets refreshed when a new offscreen image is needed */ - GeometryLayer /*!< gets refreshed everytime when a geometry changes */ - }; - - //! Layer constructor - /*! - * This is used to construct a layer. - * - * @param layername The name of the Layer - * @param mapadapter The MapAdapter which does coordinate translation and Query-String-Forming - * @param layertype The above explained LayerType - * @param takeevents Should the Layer receive MouseEvents? This is set to true by default. Setting it to false could - * be something like a "speed up hint" - */ - Layer(QString layername, MapAdapter* mapadapter, enum LayerType layertype, bool takeevents=true); - virtual ~Layer(); - - //! returns the layer's name - /*! - * @return the name of this layer - */ - QString layername() const; - - //! returns the layer´s MapAdapter - /*! - * This method returns the MapAdapter of this Layer, which can be useful - * to do coordinate transformations. - * @return the MapAdapter which us used by this Layer - */ - const MapAdapter* mapadapter() const; - - //! adds a Geometry object to this Layer - /*! - * Please notice the different LayerTypes (MapLayer and GeometryLayer) and the differences - * @param geometry the new Geometry - */ - void addGeometry(Geometry* geometry); - - void removeGeometry(Geometry* geometry); - - //! return true if the layer is visible - /** - * @return if the layer is visible - */ - bool isVisible() const; - - //! returns the LayerType of the Layer - /*! - * There are two LayerTypes: MapLayer and GeometryLayer - * @return the LayerType of this Layer - */ - Layer::LayerType layertype() const; - - void setMapAdapter(MapAdapter* mapadapter); - - Layer& operator=(const Layer& rhs); - Layer(const Layer& old); - - private: - void moveWidgets(const QPoint mapmiddle_px) const; - void drawYourImage(QPainter* painter, const QPoint mapmiddle_px) const; - void drawYourGeometries(QPainter* painter, const QPoint mapmiddle_px, QRect viewport) const; - void setSize(QSize size); - QRect offscreenViewport() const; - bool takesMouseEvents() const; - void mouseEvent(const QMouseEvent*, const QPoint mapmiddle_px); - void zoomIn() const; - void zoomOut() const; - void _draw(QPainter* painter, const QPoint mapmiddle_px) const; - - bool visible; - QString mylayername; - LayerType mylayertype; - QSize size; - QPoint screenmiddle; - - QList geometries; - MapAdapter* mapAdapter; - bool takeevents; - mutable QRect myoffscreenViewport; - - - - signals: - //! This signal is emitted when a Geometry is clicked - /*! - * A Geometry is clickable, if the containing layer is clickable. - * The layer emits a signal for every clicked geometry - * @param geometry The clicked Geometry - * @param point The coordinate (in widget coordinates) of the click - */ - void geometryClicked(Geometry* geometry, QPoint point); - - void updateRequest(QRectF rect); - void updateRequest(); - - public slots: - //! if visible is true, the layer is made visible - /*! - * @param visible if the layer should be visible - */ - void setVisible(bool visible); - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/layermanager.cpp b/src/lib/qmapcontrol/src/layermanager.cpp deleted file mode 100644 index b30ad02895773952d0cf5a47e76f1c141f6ea2cb..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/layermanager.cpp +++ /dev/null @@ -1,451 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "layermanager.h" -namespace qmapcontrol -{ - LayerManager::LayerManager(MapControl* mapcontrol, QSize size) - :mapcontrol(mapcontrol), scroll(QPoint(0,0)), size(size), whilenewscroll(QPoint(0,0)) - { -// genauer berechnen? - offSize = size + QSize(256*2, 256*2); - composedOffscreenImage = QPixmap(offSize); - composedOffscreenImage2 = QPixmap(offSize); - zoomImage = QPixmap(size); - zoomImage.fill(Qt::white); - - screenmiddle = QPoint(size.width()/2, size.height()/2); - } - - - LayerManager::~LayerManager() - { - mylayers.clear(); - } - - QPointF LayerManager::currentCoordinate() const - { - return mapmiddle; - } - - QPixmap LayerManager::getImage() const - { - return composedOffscreenImage; - } - - Layer* LayerManager::layer() const - { - Q_ASSERT_X(mylayers.size()>0, "LayerManager::getLayer()", "No layers were added!"); - return mylayers.first(); - } - - Layer* LayerManager::layer(const QString& layername) const - { - QListIterator layerit(mylayers); - while (layerit.hasNext()) - { - Layer* l = layerit.next(); - if (l->layername() == layername) - return l; - } - return 0; - } - - QList LayerManager::layers() const - { - QList keys; - QListIterator layerit(mylayers); - while (layerit.hasNext()) - { - keys.append(layerit.next()->layername()); - } - return keys; - } - - - void LayerManager::scrollView(const QPoint& point) - { -// if (scrollMutex.tryLock()) - { - - scroll += point; - zoomImageScroll+=point; - mapmiddle_px += point; -// scrollMutex.unlock(); - } - mapmiddle = layer()->mapadapter()->displayToCoordinate(mapmiddle_px); - if (!checkOffscreen()) - { - - newOffscreenImage(); - } - else - { - moveWidgets(); - } - } - - void LayerManager::moveWidgets() - { - QListIterator it(mylayers); - while (it.hasNext()) - { - it.next()->moveWidgets(mapmiddle_px); - } - } - - void LayerManager::setView(const QPointF& coordinate) - { - mapmiddle_px = layer()->mapadapter()->coordinateToDisplay(coordinate); - mapmiddle = coordinate; - - //TODO: muss wegen moveTo() raus - if (!checkOffscreen()) - { - newOffscreenImage(); - } - else - { - //TODO: - // verschiebung ausrechnen - // oder immer neues offscreenimage - newOffscreenImage(); - } - } - - void LayerManager::setView(QList coordinates) - { - setMiddle(coordinates); -// mapcontrol->update(); - } - - void LayerManager::setViewAndZoomIn(const QList coordinates) - { - while (containsAll(coordinates)) - { - setMiddle(coordinates); - zoomIn(); - } - - - if (!containsAll(coordinates)) - { - zoomOut(); - } - - mapcontrol->update(); - } - - void LayerManager::setMiddle(QList coordinates) - { - int sum_x = 0; - int sum_y = 0; - for (int i=0; imapadapter()->coordinateToDisplay(coordinates.at(i)); - sum_x += p.x(); - sum_y += p.y(); - } - QPointF middle = layer()->mapadapter()->displayToCoordinate(QPoint(sum_x/coordinates.size(), sum_y/coordinates.size())); - // middle in px rechnen! - - setView(middle); - } - - bool LayerManager::containsAll(QList coordinates) const - { - QRectF bb = getViewport(); - bool containsall = true; - for (int i=0; imapadapter()->displayToCoordinate(upperLeft); - QPointF lrCoord = layer()->mapadapter()->displayToCoordinate(lowerRight); - - QRectF coordinateBB = QRectF(ulCoord, QSizeF( (lrCoord-ulCoord).x(), (lrCoord-ulCoord).y())); - return coordinateBB; - } - - void LayerManager::addLayer(Layer* layer) - { - mylayers.append(layer); - - layer->setSize(size); - - connect(layer, SIGNAL(updateRequest(QRectF)), - this, SLOT(updateRequest(QRectF))); - connect(layer, SIGNAL(updateRequest()), - this, SLOT(updateRequest())); - - if (mylayers.size()==1) - { - setView(QPointF(0,0)); - } - } - - void LayerManager::newOffscreenImage(bool clearImage, bool showZoomImage) - { -// qDebug() << "LayerManager::newOffscreenImage()"; -// if (refreshMutex.tryLock()) - { - QPainter painter(&composedOffscreenImage2); - whilenewscroll = mapmiddle_px; - - if (clearImage) - { - composedOffscreenImage2.fill(Qt::white); - } - if (showZoomImage) - { - painter.drawPixmap(screenmiddle.x()-zoomImageScroll.x(), screenmiddle.y()-zoomImageScroll.y(),zoomImage); - } - //only draw basemaps - for (int i=0; iisVisible()) - { - if (l->layertype() == Layer::MapLayer) - { - l->drawYourImage(&painter, whilenewscroll); - } - } - } - - composedOffscreenImage = composedOffscreenImage2; -// if (scrollMutex.tryLock()) - { - scroll = mapmiddle_px-whilenewscroll; -// scroll = QPoint(0,0); -// scrollMutex.unlock(); - } - mapcontrol->update(); -// refreshMutex.unlock(); - } - - } - - void LayerManager::zoomIn() - { - QCoreApplication::processEvents(); -// if (getLayer()->getMapAdapter()->getZoom() < getLayer - ImageManager::instance()->abortLoading(); - // layer rendern abbrechen? - zoomImageScroll = QPoint(0,0); - - zoomImage.fill(Qt::white); - QPixmap tmpImg = composedOffscreenImage.copy(screenmiddle.x()+scroll.x(),screenmiddle.y()+scroll.y(), size.width(), size.height()); - - QPainter painter(&zoomImage); - painter.save(); - painter.translate(screenmiddle); - painter.scale(2, 2); - painter.translate(-screenmiddle); - - painter.drawPixmap(0,0,tmpImg); - painter.restore(); - - QListIterator it(mylayers); - //TODO: remove hack, that mapadapters wont get set zoom multiple times - QList doneadapters; - while (it.hasNext()) - { - Layer* l = it.next(); - if (!doneadapters.contains(l->mapadapter())) - { - l->zoomIn(); - doneadapters.append(l->mapadapter()); - } - } - mapmiddle_px = layer()->mapadapter()->coordinateToDisplay(mapmiddle); - whilenewscroll = mapmiddle_px; -// zoomImageScroll = mapmiddle_px; - newOffscreenImage(); - - } - - bool LayerManager::checkOffscreen() const - { - // calculate offscreenImage dimension (px) - QPoint upperLeft = mapmiddle_px - screenmiddle; - QPoint lowerRight = mapmiddle_px + screenmiddle; - QRect viewport = QRect(upperLeft, lowerRight); - - QRect testRect = layer()->offscreenViewport(); - - if (!testRect.contains(viewport)) - { - return false; - } - - return true; - } - void LayerManager::zoomOut() - { - QCoreApplication::processEvents(); - ImageManager::instance()->abortLoading(); - zoomImageScroll = QPoint(0,0); - zoomImage.fill(Qt::white); - QPixmap tmpImg = composedOffscreenImage.copy(screenmiddle.x()+scroll.x(),screenmiddle.y()+scroll.y(), size.width(), size.height()); - QPainter painter(&zoomImage); - painter.translate(screenmiddle); - painter.scale(0.5,0.5); - painter.translate(-screenmiddle); - painter.drawPixmap(0,0,tmpImg); - - painter.translate(screenmiddle); - painter.scale(2,2); - painter.translate(-screenmiddle); - - QListIterator it(mylayers); - //TODO: remove hack, that mapadapters wont get set zoom multiple times - QList doneadapters; - while (it.hasNext()) - { - Layer* l = it.next(); - if (!doneadapters.contains(l->mapadapter())) - { - l->zoomOut(); - doneadapters.append(l->mapadapter()); - } - } - mapmiddle_px = layer()->mapadapter()->coordinateToDisplay(mapmiddle); - whilenewscroll = mapmiddle_px; - newOffscreenImage(); - } - - void LayerManager::setZoom(int zoomlevel) - { - int current_zoom; - if (layer()->mapadapter()->minZoom() < layer()->mapadapter()->maxZoom()) - { - current_zoom = layer()->mapadapter()->currentZoom(); - } - else - { - current_zoom = layer()->mapadapter()->minZoom() - layer()->mapadapter()->currentZoom(); - } - - - if (zoomlevel < current_zoom) - { - for (int i=current_zoom; i>zoomlevel; i--) - { - zoomOut(); - } - } - else - { - for (int i=current_zoom; i it(mylayers); - while (it.hasNext()) - { - Layer* l = it.next(); - if (l->isVisible()) - { - l->mouseEvent(evnt, mapmiddle_px); - } - } - } - - void LayerManager::updateRequest(QRectF rect) - { - const QPoint topleft = mapmiddle_px - screenmiddle; - - QPointF c = rect.topLeft(); - - if (getViewport().contains(c) || getViewport().contains(rect.bottomRight())) - { -// QPoint point = getLayer()->getMapAdapter()->coordinateToDisplay(c); -// QPoint finalpoint = point-topleft; -// QRect rect_px = QRect(int(finalpoint.x()-(rect.width()-1)/2), int(finalpoint.y()-(rect.height()-1)/2), -// int(rect.width()+1), int(rect.height()+1)); - // -// mapcontrol->updateRequest(rect_px); - mapcontrol->update(); -// newOffscreenImage(); - } - } - void LayerManager::updateRequest() - { - newOffscreenImage(); - } - void LayerManager::forceRedraw() - { - newOffscreenImage(); - } - void LayerManager::removeZoomImage() - { - zoomImage.fill(Qt::white); - forceRedraw(); - } - - void LayerManager::drawGeoms(QPainter* painter) - { - QListIterator it(mylayers); - while (it.hasNext()) - { - Layer* l = it.next(); - if (l->layertype() == Layer::GeometryLayer && l->isVisible()) - { - l->drawYourGeometries(painter, mapmiddle_px, layer()->offscreenViewport()); - } - } - } - void LayerManager::drawImage(QPainter* painter) - { - painter->drawPixmap(-scroll.x()-screenmiddle.x(), - -scroll.y()-screenmiddle.y(), - composedOffscreenImage); - } - - int LayerManager::currentZoom() const - { - return layer()->mapadapter()->currentZoom(); - } -} diff --git a/src/lib/qmapcontrol/src/layermanager.h b/src/lib/qmapcontrol/src/layermanager.h deleted file mode 100644 index ede653126a7c09d95b2a95052cedfdaa5505be9c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/layermanager.h +++ /dev/null @@ -1,210 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef LAYERMANAGER_H -#define LAYERMANAGER_H - -#include -#include -#include "layer.h" -#include "mapadapter.h" -#include "mapcontrol.h" - -namespace qmapcontrol -{ - class Layer; - class MapAdapter; - class MapControl; - - class LayerManager; - - //! Handles Layers and viewport related settings - /*! - * This class handles internally all layers which were added to the MapControl. - * It also stores values for scrolling. - * It initiates the creation of a new offscreen image and on zooming the zoom images gets here created. - * @author Kai Winter - */ - class LayerManager : public QObject - { - Q_OBJECT - public: - LayerManager(MapControl*, QSize); - ~LayerManager(); - - //! returns the coordinate of the center of the map - /*! - * @return returns the coordinate of the middle of the screen - */ - QPointF currentCoordinate () const; - - //! returns the current offscreen image - /*! - * @return the current offscreen image - */ - QPixmap getImage () const; - - //! returns the layer with the given name - /*! - * @param layername name of the wanted layer - * @return the layer with the given name - */ - Layer* layer (const QString&) const; - - //! returns the base layer - /*! - * This will return the base layer of the LayerManager. - * The base layer is the one which is used to do internal coordinate calculations. - * @return the base layer - */ - Layer* layer () const; - - //! returns the names of all layers - /*! - * @return returns a QList with the names of all layers - */ - QList layers () const; - - //! sets the middle of the map to the given coordinate - /*! - * @param coordinate the coordinate which the view´s middle should be set to - */ - void setView (const QPointF& coordinate); - - //! sets the view, so all coordinates are visible - /*! - * @param coordinates the Coorinates which should be visible - */ - void setView (const QList coordinates); - - //! sets the view and zooms in, so all coordinates are visible - /*! - * The code of setting the view to multiple coordinates is "brute force" and pretty slow. - * Have to be reworked. - * @param coordinates the Coorinates which should be visible - */ - void setViewAndZoomIn (const QList coordinates); - - //! zooms in one step - void zoomIn (); - - //! zooms out one step - void zoomOut (); - - //! sets the given zoomlevel - /*! - * @param zoomlevel the zoomlevel - */ - void setZoom(int zoomlevel); - - //! The Viewport of the display - /*! - * Returns the visible viewport in world coordinates - * @return the visible viewport in world coordinates - */ - QRectF getViewport() const; - - //! scrolls the view - /*! - * Scrolls the view by the given value in pixels and in display coordinates - * @param offset the distance which the view should be scrolled - */ - void scrollView(const QPoint& offset); - - //! forwards mouseevents to the layers - /*! - * This method is invoked by the MapControl which receives Mouse Events. - * These events are forwarded to the layers, so they can check for clicked geometries. - * @param evnt the mouse event - */ - void mouseEvent(const QMouseEvent* evnt); - - //! returns the middle of the map in projection coordinates - /*! - * - * @return the middle of the map in projection coordinates - */ - QPoint getMapmiddle_px() const; - - void forceRedraw(); - void removeZoomImage(); - - //! adds a layer - /*! - * If multiple layers are added, they are painted in the added order. - * @param layer the layer which should be added - */ - void addLayer(Layer* layer); - - //! returns the current zoom level - /*! - * @return returns the current zoom level - */ - int currentZoom() const; - - void drawGeoms(QPainter* painter); - void drawImage(QPainter* painter); - - private: - LayerManager& operator=(const LayerManager& rhs); - LayerManager(const LayerManager& old); - //! This method have to be invoked to draw a new offscreen image - /*! - * @param clearImage if the current offscreeen image should be cleared - * @param showZoomImage if a zoom image should be painted - */ - void newOffscreenImage(bool clearImage=true, bool showZoomImage=true); - inline bool checkOffscreen() const; - inline bool containsAll(QList coordinates) const; - inline void moveWidgets(); - inline void setMiddle(QList coordinates); - - MapControl* mapcontrol; - QPoint screenmiddle; // middle of the screen - QPoint scroll; // scrollvalue of the offscreen image - QPoint zoomImageScroll; // scrollvalue of the zoom image - - QSize size; // widget size - QSize offSize; // size of the offscreen image - - QPixmap composedOffscreenImage; - QPixmap composedOffscreenImage2; - QPixmap zoomImage; - - QList mylayers; - - QPoint mapmiddle_px; // projection-display coordinates - QPointF mapmiddle; // world coordinate - - QMutex scrollMutex; - QPoint whilenewscroll; - mutable QMutex refreshMutex; - - public slots: - void updateRequest(QRectF rect); - void updateRequest(); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/linestring.cpp b/src/lib/qmapcontrol/src/linestring.cpp deleted file mode 100644 index 1cd39b1f939059072bd32b38ab8c7d127a024611..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/linestring.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "linestring.h" -namespace qmapcontrol -{ - LineString::LineString() - : Curve() - { - GeometryType = "LineString"; - } - - LineString::LineString(QList const points, QString name, QPen* pen) - :Curve(name) - { - mypen = pen; - LineString(); - setPoints(points); - - //TODO: bremst stark -// pen.setStyle(Qt::DashDotDotLine); - } - - LineString::~LineString() - { - } - -// Geometry LineString::Clone(){} - -// Point LineString::EndPoint(){} -// Point LineString::StartPoint(){} -// Point LineString::Value(){} - - - void LineString::addPoint(Point* point) - { - vertices.append(point); - } - - QList LineString::points() - { - return vertices; - } - - void LineString::setPoints(QList points) - { - for (int i=0; isetParentGeometry(this); - } - vertices = points; - } - - void LineString::draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &screensize, const QPoint offset) - { - if (!visible) - return; - - QPolygon p = QPolygon(); - - QPointF c; - for (int i=0; icoordinate(); - p.append(mapadapter->coordinateToDisplay(c)); - } - if (mypen != 0) - { - painter->save(); - painter->setPen(*mypen); - } - painter->drawPolyline(p); - if (mypen != 0) - { - painter->restore(); - } - for (int i=0; idraw(painter, mapadapter, screensize, offset); - } - } - - int LineString::numberOfPoints() const - { - return vertices.count(); - } - bool LineString::Touches(Point* geom, const MapAdapter* mapadapter) - { -// qDebug() << "LineString::Touches Point"; - touchedPoints.clear(); - bool touches = false; - for (int i=0; iTouches(geom, mapadapter)) - { - touchedPoints.append(vertices.at(i)); - - touches = true; - } - } - if (touches) - { - emit(geometryClicked(this, QPoint(0,0))); - } - return touches; - } - bool LineString::Touches(Geometry* /*geom*/, const MapAdapter* /*mapadapter*/) - { -// qDebug() << "LineString::Touches Geom"; - touchedPoints.clear(); - - return false; - } - - QList LineString::clickedPoints() - { - return touchedPoints; - } - bool LineString::hasPoints() const - { - return vertices.size() > 0 ? true : false; - } - bool LineString::hasClickedPoints() const - { - return touchedPoints.size() > 0 ? true : false; - } - - QRectF LineString::boundingBox() - { - qreal minlon=180; - qreal maxlon=-180; - qreal minlat=90; - qreal maxlat=-90; - for (int i=0; ilongitude() < minlon) minlon = tmp->longitude(); - if (tmp->longitude() > maxlon) maxlon = tmp->longitude(); - if (tmp->latitude() < minlat) minlat = tmp->latitude(); - if (tmp->latitude() > maxlat) maxlat = tmp->latitude(); - } - QPointF min = QPointF(minlon, minlat); - QPointF max = QPointF(maxlon, maxlat); - QPointF dist = max - min; - QSizeF si = QSizeF(dist.x(), dist.y()); - - return QRectF(min, si); - - } -} diff --git a/src/lib/qmapcontrol/src/linestring.h b/src/lib/qmapcontrol/src/linestring.h deleted file mode 100644 index 0ac95b1f38234870e1e838d0635796d3c1387eb4..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/linestring.h +++ /dev/null @@ -1,120 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef LINESTRING_H -#define LINESTRING_H - -#include "curve.h" - -namespace qmapcontrol -{ - //! A collection of Point objects to describe a line - /*! - * A LineString is a Curve with linear interpolation between Points. Each consecutive pair of Points defines a Line segment. - * @author Kai Winter - */ - class LineString : public Curve - { - Q_OBJECT - public: - LineString(); - //! constructor - /*! - * The constructor of a LineString takes a list of Points to form a line. - * @param points a list of points - * @param name the name of the LineString - * @param pen a QPen can be used to modify the look of the line. - * @see http://doc.trolltech.com/4.3/qpen.html - */ - LineString ( QList const points, QString name = QString(), QPen* pen = 0 ); - virtual ~LineString(); - - //! returns the points of the LineString - /*! - * @return a list with the points of the LineString - */ - QList points(); - - //! adds a point at the end of the LineString - /*! - * @param point the point which should be added to the LineString - */ - void addPoint ( Point* point ); - - //! sets the given list as points of the LineString - /*! - * @param points the points which should be set for the LineString - */ - void setPoints ( QList points ); - - //! returns the number of Points the LineString consists of - /*! - * @return the number of the LineString´s Points - */ - int numberOfPoints() const; - -// virtual Geometry Clone(); - virtual QRectF boundingBox(); -// virtual Point EndPoint(); -// virtual Point StartPoint(); -// virtual Point Value(); - - //! returns true if the LineString has Childs - /*! - * This is equal to: numberOfPoints() > 0 - * @return true it the LineString has Childs (=Points) - * @see clickedPoints() - */ - virtual bool hasPoints() const; - - //! returns true if the LineString has clicked Points - /*! - * @return true if childs of a LineString were clicked - * @see clickedPoints() - */ - virtual bool hasClickedPoints() const; - - //! returns the clicked Points - /*! - * If a LineString was clicked it could be neccessary to figure out which of its points where clicked. - * Do do so the methods hasPoints() and clickedPoints() can be used. - * When a point is added to a LineString the Point becomes its child. - * It is possible (depending on the zoomfactor) to click more than one Point of a LineString, so this method returns a list. - * @return the clicked Points of the LineString - */ - virtual QList clickedPoints(); - - protected: - virtual bool Touches ( Geometry* geom, const MapAdapter* mapadapter ); - virtual bool Touches ( Point* geom, const MapAdapter* mapadapter ); - virtual void draw ( QPainter* painter, const MapAdapter* mapadapter, const QRect &screensize, const QPoint offset ); - - private: - QList vertices; - QList touchedPoints; - - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/mapadapter.cpp b/src/lib/qmapcontrol/src/mapadapter.cpp deleted file mode 100644 index 607a3a775114f9ed515471cf528d034ea70e0ce6..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapadapter.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "mapadapter.h" -namespace qmapcontrol -{ - MapAdapter::MapAdapter(const QString& host, const QString& serverPath, int tilesize, int minZoom, int maxZoom) - :myhost(host), serverPath(serverPath), mytilesize(tilesize), min_zoom(minZoom), max_zoom(maxZoom) - { - current_zoom = min_zoom; - loc = QLocale(QLocale::English); - } - - MapAdapter::~MapAdapter() - { - } - - QString MapAdapter::host() const - { - return myhost; - } - - int MapAdapter::tilesize() const - { - return mytilesize; - } - - int MapAdapter::minZoom() const - { - return min_zoom; - } - - int MapAdapter::maxZoom() const - { - return max_zoom; - } - - int MapAdapter::currentZoom() const - { - return current_zoom; - } - - int MapAdapter::adaptedZoom() const - { - return max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - } -} diff --git a/src/lib/qmapcontrol/src/mapadapter.h b/src/lib/qmapcontrol/src/mapadapter.h deleted file mode 100644 index ea9b6aa8457b5ebd910bb303bc64c77e5e033b74..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapadapter.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef MAPADAPTER_H -#define MAPADAPTER_H - -#include -#include -#include -#include -#include -#include -#include - -namespace qmapcontrol -{ - //! Used to fit map servers into QMapControl - /*! - * MapAdapters are needed to convert between world- and display coordinates. - * This calculations depend on the used map projection. - * There are two ready-made MapAdapters: - * - TileMapAdapter, which is ready to use for OpenStreetMap or Google (Mercator projection) - * - WMSMapAdapter, which could be used for the most WMS-Server (some servers show errors, because of image ratio) - * - * MapAdapters are also needed to form the HTTP-Queries to load the map tiles. - * The maps from WMS Servers are also divided into tiles, because those can be better cached. - * - * @see TileMapAdapter, @see WMSMapAdapter - * - * @author Kai Winter - */ - class MapAdapter : public QObject - { - friend class Layer; - - Q_OBJECT - public: - virtual ~MapAdapter(); - - //! returns the host of this MapAdapter - /*! - * @return the host of this MapAdapter - */ - QString host () const; - - //! returns the size of the tiles - /*! - * @return the size of the tiles - */ - int tilesize () const; - - //! returns the min zoom value - /*! - * @return the min zoom value - */ - int minZoom () const; - - //! returns the max zoom value - /*! - * @return the max zoom value - */ - int maxZoom () const; - - //! returns the current zoom - /*! - * @return the current zoom - */ - int currentZoom () const; - - virtual int adaptedZoom()const; - - - //! translates a world coordinate to display coordinate - /*! - * The calculations also needs the current zoom. The current zoom is managed by the MapAdapter, so this is no problem. - * To divide model from view the current zoom should be moved to the layers. - * @param coordinate the world coordinate - * @return the display coordinate (in widget coordinates) - */ - virtual QPoint coordinateToDisplay(const QPointF& coordinate) const = 0; - - //! translates display coordinate to world coordinate - /*! - * The calculations also needs the current zoom. The current zoom is managed by the MapAdapter, so this is no problem. - * To divide model from view the current zoom should be moved to the layers. - * @param point the display coordinate - * @return the world coordinate - */ - virtual QPointF displayToCoordinate(const QPoint& point) const = 0; - - protected: - MapAdapter(const QString& host, const QString& serverPath, int tilesize, int minZoom = 0, int maxZoom = 0); - virtual void zoom_in() = 0; - virtual void zoom_out() = 0; - virtual bool isValid(int x, int y, int z) const = 0; - virtual QString query(int x, int y, int z) const = 0; - - QSize size; - QString myhost; - QString serverPath; - int mytilesize; - int min_zoom; - int max_zoom; - int current_zoom; - - int param1; - int param2; - int param3; - int param4; - int param5; - int param6; - - QString sub1; - QString sub2; - QString sub3; - QString sub4; - QString sub5; - QString sub6; - - int order[3][2]; - - int middle_x; - int middle_y; - - qreal numberOfTiles; - QLocale loc; - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/mapcontrol.cpp b/src/lib/qmapcontrol/src/mapcontrol.cpp deleted file mode 100644 index 9b20a8d46275e700406412e763ac35eadbb777de..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapcontrol.cpp +++ /dev/null @@ -1,409 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "mapcontrol.h" -namespace qmapcontrol -{ - MapControl::MapControl(QSize size, MouseMode mousemode) - : size(size), mymousemode(mousemode), scaleVisible(false) - { - layermanager = new LayerManager(this, size); - screen_middle = QPoint(size.width()/2, size.height()/2); - - mousepressed = false; - - connect(ImageManager::instance(), SIGNAL(imageReceived()), - this, SLOT(updateRequestNew())); - - connect(ImageManager::instance(), SIGNAL(loadingFinished()), - this, SLOT(loadingFinished())); - - this->setMaximumSize(size.width()+1, size.height()+1); - } - - MapControl::~MapControl() - { - delete layermanager; - } - - QPointF MapControl::currentCoordinate() const - { - return layermanager->currentCoordinate(); - } - - Layer* MapControl::layer(const QString& layername) const - { - return layermanager->layer(layername); - } - - QList MapControl::layers() const - { - return layermanager->layers(); - } - - int MapControl::numberOfLayers() const - { - return layermanager->layers().size(); - } - - void MapControl::followGeometry(const Geometry* geom) const - { - connect(geom, SIGNAL(positionChanged(Geometry*)), - this, SLOT(positionChanged(Geometry*))); - } - - void MapControl::positionChanged(Geometry* geom) - { - QPoint start = layermanager->layer()->mapadapter()->coordinateToDisplay(currentCoordinate()); - QPoint dest = layermanager->layer()->mapadapter()->coordinateToDisplay(((Point*)geom)->coordinate()); - - QPoint step = (dest-start); - - layermanager->scrollView(step); - -// setView(geom); - update(); - } - - void MapControl::moveTo(QPointF coordinate) - { - target = coordinate; - steps = 25; - if (moveMutex.tryLock()) - { - QTimer::singleShot(40, this, SLOT(tick())); - } - else - { -// stopMove(coordinate); - } - } - void MapControl::tick() - { - QPoint start = layermanager->layer()->mapadapter()->coordinateToDisplay(currentCoordinate()); - QPoint dest = layermanager->layer()->mapadapter()->coordinateToDisplay(target); - - QPoint step = (dest-start)/steps; - QPointF next = currentCoordinate()- step; - -// setView(Coordinate(next.x(), next.y())); - layermanager->scrollView(step); - - update(); - steps--; - if (steps>0) - { - QTimer::singleShot(40, this, SLOT(tick())); - } - else - { - moveMutex.unlock(); - } - } - - void MapControl::paintEvent(QPaintEvent* evnt) - { - QWidget::paintEvent(evnt); - QPainter painter(this); - -// painter.translate(150,190); -// painter.scale(0.5,0.5); - -// painter.setClipRect(0,0, size.width(), size.height()); - -// painter.setViewport(10000000000,0,size.width(),size.height()); - - /* - // rotating - rotation = 45; - painter.translate(256,256); - painter.rotate(rotation); - painter.translate(-256,-256); - */ - - layermanager->drawImage(&painter); - layermanager->drawGeoms(&painter); - - // added by wolf - // draw scale - if (scaleVisible) - { - QList distanceList; - distanceList<<5000000<<2000000<<1000000<<1000000<<1000000<<100000<<100000<<50000<<50000<<10000<<10000<<10000<<1000<<1000<<500<<200<<100<<50<<25; - if (currentZoom() >= 0 && distanceList.size() > currentZoom()) - { - double line; - line = distanceList.at( currentZoom() ) / pow(2, 18-currentZoom() ) / 0.597164; - - // draw the scale - painter.setPen(Qt::black); - QPoint p1(10,size.height()-20); - QPoint p2((int)line,size.height()-20); - painter.drawLine(p1,p2); - - painter.drawLine(10,size.height()-15, 10,size.height()-25); - painter.drawLine((int)line,size.height()-15, (int)line,size.height()-25); - - QString distance; - if (distanceList.at(currentZoom()) >= 1000) - { - distance = QVariant( distanceList.at(currentZoom())/1000 ) .toString()+ " km"; - } - else - { - distance = QVariant( distanceList.at(currentZoom()) ).toString() + " m"; - } - - painter.drawText(QPoint((int)line+10,size.height()-15), distance); - } - } - - - - painter.drawLine(screen_middle.x(), screen_middle.y()-10, - screen_middle.x(), screen_middle.y()+10); // | - painter.drawLine(screen_middle.x()-10, screen_middle.y(), - screen_middle.x()+10, screen_middle.y()); // - - -// int cross_x = int(layermanager->getMapmiddle_px().x())%256; -// int cross_y = int(layermanager->getMapmiddle_px().y())%256; -// painter.drawLine(screen_middle.x()-cross_x+cross_x, screen_middle.y()-cross_y+0, -// screen_middle.x()-cross_x+cross_x, screen_middle.y()-cross_y+256); // | -// painter.drawLine(screen_middle.x()-cross_x+0, screen_middle.y()-cross_y+cross_y, -// screen_middle.x()-cross_x+256, screen_middle.y()-cross_y+cross_y); // - - - painter.drawRect(0,0, size.width(), size.height()); - /* - // rotating - painter.setMatrix(painter.matrix().inverted()); - //qt = painter.transform(); - qm = painter.combinedMatrix(); - */ - - if (mousepressed && mymousemode == Dragging) - { - QRect rect = QRect(pre_click_px, current_mouse_pos); - painter.drawRect(rect); - } - } - -// mouse events - void MapControl::mousePressEvent(QMouseEvent* evnt) - { - //rotating - -// QMouseEvent* me = new QMouseEvent(evnt->type(), qm.map(QPoint(evnt->x(),evnt->y())), evnt->button(), evnt->buttons(), evnt->modifiers()); -// evnt = me; -// qDebug() << "evnt: " << evnt->x() << ", " << evnt->y() << ", " << evnt->pos(); - - - layermanager->mouseEvent(evnt); - - if (layermanager->layers().size()>0) - { - if (evnt->button() == 1) - { - mousepressed = true; - pre_click_px = QPoint(evnt->x(), evnt->y()); - } - else if (evnt->button() == 2 && mymousemode != None) // zoom in - { - zoomIn(); - } else if (evnt->button() == 4 && mymousemode != None) // zoom out - { - zoomOut(); - } - } - -// emit(mouseEvent(evnt)); - emit(mouseEventCoordinate(evnt, clickToWorldCoordinate(evnt->pos()))); - } - - void MapControl::mouseReleaseEvent(QMouseEvent* evnt) - { - mousepressed = false; - if (mymousemode == Dragging) - { - QPointF ulCoord = clickToWorldCoordinate(pre_click_px); - QPointF lrCoord = clickToWorldCoordinate(current_mouse_pos); - - QRectF coordinateBB = QRectF(ulCoord, QSizeF( (lrCoord-ulCoord).x(), (lrCoord-ulCoord).y())); - - emit(boxDragged(coordinateBB)); - } - - emit(mouseEventCoordinate(evnt, clickToWorldCoordinate(evnt->pos()))); - } - - void MapControl::mouseMoveEvent(QMouseEvent* evnt) - { -// emit(mouseEvent(evnt)); - - /* - // rotating - QMouseEvent* me = new QMouseEvent(evnt->type(), qm.map(QPoint(evnt->x(),evnt->y())), evnt->button(), evnt->buttons(), evnt->modifiers()); - evnt = me; - */ - if (mousepressed && mymousemode == Panning) - { - QPoint offset = pre_click_px - QPoint(evnt->x(), evnt->y()); - layermanager->scrollView(offset); - pre_click_px = QPoint(evnt->x(), evnt->y()); - } - else if (mousepressed && mymousemode == Dragging) - { - current_mouse_pos = QPoint(evnt->x(), evnt->y()); - } -// emit(mouseEventCoordinate(evnt, clickToWorldCoordinate(evnt->pos()))); - - update(); -// emit(mouseEventCoordinate(evnt, clickToWorldCoordinate(evnt->pos()))); - } - - QPointF MapControl::clickToWorldCoordinate(QPoint click) - { - // click coordinate to image coordinate - QPoint displayToImage= QPoint(click.x()-screen_middle.x()+layermanager->getMapmiddle_px().x(), - click.y()-screen_middle.y()+layermanager->getMapmiddle_px().y()); - // image coordinate to world coordinate - return layermanager->layer()->mapadapter()->displayToCoordinate(displayToImage); - } - - void MapControl::updateRequest(QRect rect) - { - update(rect); - } - void MapControl::updateRequestNew() - { -// qDebug() << "MapControl::updateRequestNew()"; - layermanager->forceRedraw(); - update(); - } -// slots - void MapControl::zoomIn() - { - layermanager->zoomIn(); - update(); - } - void MapControl::zoomOut() - { - layermanager->zoomOut(); - update(); - } - void MapControl::setZoom(int zoomlevel) - { - layermanager->setZoom(zoomlevel); - update(); - } - int MapControl::currentZoom() const - { - return layermanager->currentZoom(); - } - void MapControl::scrollLeft(int pixel) - { - layermanager->scrollView(QPoint(-pixel,0)); - update(); - } - void MapControl::scrollRight(int pixel) - { - layermanager->scrollView(QPoint(pixel,0)); - update(); - } - void MapControl::scrollUp(int pixel) - { - layermanager->scrollView(QPoint(0,-pixel)); - update(); - } - void MapControl::scrollDown(int pixel) - { - layermanager->scrollView(QPoint(0,pixel)); - update(); - } - void MapControl::scroll(const QPoint scroll) - { - layermanager->scrollView(scroll); - update(); - } - - void MapControl::setView(const QPointF& coordinate) const - { - layermanager->setView(coordinate); - } - - void MapControl::setView(const QList coordinates) const - { - layermanager->setView(coordinates); - } - - void MapControl::setViewAndZoomIn(const QList coordinates) const - { - layermanager->setViewAndZoomIn(coordinates); - } - - void MapControl::setView(const Point* point) const - { - layermanager->setView(point->coordinate()); - } - - void MapControl::loadingFinished() - { -// qDebug() << "MapControl::loadingFinished()"; - layermanager->removeZoomImage(); - } - void MapControl::addLayer(Layer* layer) - { - layermanager->addLayer(layer); - } - - void MapControl::setMouseMode(MouseMode mousemode) - { - mymousemode = mousemode; - } - MapControl::MouseMode MapControl::mouseMode() - { - return mymousemode; - } - - void MapControl::stopFollowing(Geometry* geom) - { - geom->disconnect(SIGNAL(positionChanged(Geometry*))); - } - - void MapControl::enablePersistentCache(const QDir& path) - { - ImageManager::instance()->setCacheDir(path); - } - - void MapControl::setProxy(QString host, int port) - { - ImageManager::instance()->setProxy(host, port); - } - - void MapControl::showScale(bool show) - { - scaleVisible = show; - } - -} diff --git a/src/lib/qmapcontrol/src/mapcontrol.h b/src/lib/qmapcontrol/src/mapcontrol.h deleted file mode 100644 index 7dbe166fbf1b9e8f38fb1477982097819f49e76a..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapcontrol.h +++ /dev/null @@ -1,320 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef MAPCONTROL_H -#define MAPCONTROL_H - -#include - -#include "layermanager.h" -#include "layer.h" -#include "mapadapter.h" -#include "geometry.h" -#include "imagemanager.h" - -//! QMapControl namespace -namespace qmapcontrol -{ - class LayerManager; - class MapAdapter; - class Layer; - - //! The control element of the widget and also the widget itself - /*! - * This is the main widget. - * To this control layers can be added. - * A MapControl have to be instantiated with a QSize which sets the size the widget takes in a layout. - * The given size is also the size, which is asured to be filled with map images. - * - * @author Kai Winter - */ - class MapControl : public QWidget - { - Q_OBJECT - - public: - //! Declares what actions the mouse move has on the map - enum MouseMode - { - Panning, /*!< The map is moved */ - Dragging, /*!< A rectangular can be drawn */ - None, /*!< Mouse move events have no efect to the map */ - }; - - //! The constructor of MapControl - /*! - * The MapControl is the widget which displays the maps. - * The size describes the area, which gets filled with map data - * When you give no MouseMode, the mouse is moving the map. - * You can change the MouseMode on runtime, to e.g. Dragging, which lets the user drag a rectangular box. - * After the dragging a signal with the size of the box is emitted. - * The mousemode ´None´ can be used, to completely define the control of the map yourself. - * @param size the size which the widget should fill with map data - * @param mousemode the way mouseevents are handled - */ - MapControl(QSize size, MouseMode mousemode = Panning); - - ~MapControl(); - - //! adds a layer - /*! - * If multiple layers are added, they are painted in the added order. - * @param layer the layer which should be added - */ - void addLayer(Layer* layer); - - //! returns the layer with the given name - /*! - * @param layername name of the wanted layer - * @return the layer with the given name - */ - Layer* layer(const QString& layername) const; - - //! returns the names of all layers - /*! - * @return returns a QList with the names of all layers - */ - QList layers() const; - - //! returns the number of existing layers - /*! - * @return returns the number of existing layers - */ - int numberOfLayers() const; - - //! returns the coordinate of the center of the map - /*! - * @return returns the coordinate of the middle of the screen - */ - QPointF currentCoordinate() const; - - //! returns the current zoom level - /*! - * @return returns the current zoom level - */ - int currentZoom() const; - - //! sets the middle of the map to the given coordinate - /*! - * @param coordinate the coordinate which the view´s middle should be set to - */ - void setView(const QPointF& coordinate) const; - - //! sets the view, so all coordinates are visible - /*! - * @param coordinates the Coorinates which should be visible - */ - void setView(const QList coordinates) const; - - //! sets the view and zooms in, so all coordinates are visible - /*! - * The code of setting the view to multiple coordinates is "brute force" and pretty slow. - * Have to be reworked. - * @param coordinates the Coorinates which should be visible - */ - void setViewAndZoomIn(const QList coordinates) const; - - //! sets the view to the given Point - /*! - * - * @param point the geometric point the view should be set to - */ - void setView(const Point* point) const; - - //! Keeps the center of the map on the Geometry, even when it moves - /*! - * To stop the following the method stopFollowing() have to be called - * @param geometry the Geometry which should stay centered. - */ - void followGeometry(const Geometry* geometry) const; - //TODO: -// void followGeometry(const QList) const; - - //! Stops the following of a Geometry - /*! - * if the view is set to follow a Geometry this method stops the trace. - * See followGeometry(). - * @param geometry the Geometry which should not followed anymore - */ - void stopFollowing(Geometry* geometry); - - //! Smoothly moves the center of the view to the given Coordinate - /*! - * @param coordinate the Coordinate which the center of the view should moved to - */ - void moveTo (QPointF coordinate); - - //! sets the Mouse Mode of the MapControl - /*! - * There are three MouseModes declard by an enum. - * The MouesMode Dragging draws an rectangular in the map while the MouseButton is pressed. - * When the Button is released a boxDragged() signal is emitted. - * - * The second MouseMode (the default) is Panning, which allows to drag the map around. - * @param mousemode the MouseMode - */ - void setMouseMode(MouseMode mousemode); - - //! returns the current MouseMode - /*! - * For a explanation for the MouseModes see setMouseMode() - * @return the current MouseMode - */ - MapControl::MouseMode mouseMode(); - - //int rotation; - - //! Enable persistent caching of map tiles - /*! - * Call this method to allow the QMapControl widget to save map tiles - * persistent (also over application restarts). - * Tiles are stored in the subdirectory "QMapControl.cache" within the - * user's home directory. This can be changed by giving a path. - * @param path the path to the cache directory - */ - void enablePersistentCache(const QDir& path=QDir::homePath() + "/QMapControl.cache"); - - - //! Sets the proxy for HTTP connections - /*! - * This method sets the proxy for HTTP connections. - * This is not provided by the current Qtopia version! - * @param host the proxy´s hostname or ip - * @param port the proxy´s port - */ - void setProxy(QString host, int port); - - //! Displays the scale within the widget - /*! - * - * @param show true if the scale should be displayed - */ - void showScale(bool show); - - private: - LayerManager* layermanager; - QPoint screen_middle; // middle of the widget (half size) - - QPoint pre_click_px; // used for scrolling (MouseMode Panning) - QPoint current_mouse_pos; // used for scrolling and dragging (MouseMode Panning/Dragging) - - QSize size; // size of the widget - - bool mousepressed; - MouseMode mymousemode; - bool scaleVisible; - - bool m_loadingFlag; - - QMutex moveMutex; // used for method moveTo() - QPointF target; // used for method moveTo() - int steps; // used for method moveTo() - - QPointF clickToWorldCoordinate(QPoint click); - MapControl& operator=(const MapControl& rhs); - MapControl(const MapControl& old); - - protected: - void paintEvent(QPaintEvent* evnt); - void mousePressEvent(QMouseEvent* evnt); - void mouseReleaseEvent(QMouseEvent* evnt); - void mouseMoveEvent(QMouseEvent* evnt); - - - signals: -// void mouseEvent(const QMouseEvent* evnt); - - //! Emitted AFTER a MouseEvent occured - /*! - * This signals allows to receive click events within the MapWidget together with the world coordinate. - * It is emitted on MousePressEvents and MouseReleaseEvents. - * The kind of the event can be obtained by checking the events type. - * @param evnt the QMouseEvent that occured - * @param coordinate the corresponding world coordinate - */ - void mouseEventCoordinate(const QMouseEvent* evnt, const QPointF coordinate); - - //! Emitted, after a Rectangular is dragged. - /*! - * It is possible to select a rectangular area in the map, if the MouseMode is set to Dragging. - * The coordinates are in world coordinates - * @param QRectF the dragged Rect - */ - void boxDragged(const QRectF); - - //! This signal is emmited, when a Geometry is clicked - /*! - * @param geometry - * @param coord_px asd - */ - void geometryClicked(Geometry* geometry, QPoint coord_px); - - public slots: - //! zooms in one step - void zoomIn(); - - //! zooms out one step - void zoomOut(); - - //! sets the given zoomlevel - /*! - * @param zoomlevel the zoomlevel - */ - void setZoom(int zoomlevel); - - //! scrolls the view to the left - void scrollLeft(int pixel=10); - - //! scrolls the view to the right - void scrollRight(int pixel=10); - - //! scrolls the view up - void scrollUp(int pixel=10); - - //! scrolls the view down - void scrollDown(int pixel=10); - - //! scrolls the view by the given point - void scroll(const QPoint scroll); - - //! updates the map for the given rect - /*! - * @param rect the area which should be repainted - */ - void updateRequest(QRect rect); - - //! updates the hole map by creating a new offscreen image - /*! - * - */ - void updateRequestNew(); - - private slots: - void tick(); - void loadingFinished(); - void positionChanged(Geometry* geom); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/maplayer.cpp b/src/lib/qmapcontrol/src/maplayer.cpp deleted file mode 100644 index 21377b73be6907e0f8fd999308e04c25ff998d8c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/maplayer.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "maplayer.h" -namespace qmapcontrol -{ - MapLayer::MapLayer(QString layername, MapAdapter* mapadapter, bool takeevents) - : Layer(layername, mapadapter, Layer::MapLayer, takeevents) - { - } - - - MapLayer::~MapLayer() - { - } -} diff --git a/src/lib/qmapcontrol/src/maplayer.h b/src/lib/qmapcontrol/src/maplayer.h deleted file mode 100644 index a66439ae3898ea818364b2e120119f23cbba2d5a..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/maplayer.h +++ /dev/null @@ -1,64 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef MAPLAYER_H -#define MAPLAYER_H - -#include "layer.h" - -namespace qmapcontrol -{ - //! MapLayer class - /*! - * There are two different layer types: - * - MapLayer: Displays Maps, but also Geometries. The configuration for displaying maps have to be done in the MapAdapter - * - GeometryLayer: Only displays Geometry objects. - * - * MapLayers also can display Geometry objects. The difference to the GeometryLayer is the repainting. Objects that are - * added to a MapLayer are "baken" on the map. This means, when you change it´s position for example the changes are - * not visible until a new offscreen image has been drawn. If you have "static" Geometries which won´t change their - * position this is fine. But if you want to change the objects position or pen you should use a GeometryLayer. Those - * are repainted immediately on changes. - * - * @author Kai Winter - */ - class MapLayer : public Layer - { - Q_OBJECT - public: - //! MapLayer constructor - /*! - * This is used to construct a map layer. - * - * @param layername The name of the Layer - * @param mapadapter The MapAdapter which does coordinate translation and Query-String-Forming - * @param takeevents Should the Layer receive MouseEvents? This is set to true by default. Setting it to false could - * be something like a "speed up hint" - */ - MapLayer(QString layername, MapAdapter* mapadapter, bool takeevents=true); - virtual ~MapLayer(); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/mapnetwork.cpp b/src/lib/qmapcontrol/src/mapnetwork.cpp deleted file mode 100644 index dcebdcc30e2f7102fb6c534569beb26739eab9c9..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapnetwork.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "mapnetwork.h" -#include -namespace qmapcontrol -{ - MapNetwork::MapNetwork(ImageManager* parent) - :parent(parent), http(new QHttp(this)), loaded(0) - { - connect(http, SIGNAL(requestFinished(int, bool)), - this, SLOT(requestFinished(int, bool))); -// http->setProxy("www-cache.mi.fh-wiesbaden.de", 8080); - } - - MapNetwork::~MapNetwork() - { - http->clearPendingRequests(); - delete http; - } - - - void MapNetwork::loadImage(const QString& host, const QString& url) - { -// qDebug() << "getting: " << QString(host).append(url); -// http->setHost(host); -// int getId = http->get(url); - - http->setHost(host); - QHttpRequestHeader header("GET", url); - header.setValue("User-Agent", "Mozilla"); - header.setValue("Host", host); - int getId = http->request(header); - - if (vectorMutex.tryLock()) - { - loadingMap[getId] = url; - vectorMutex.unlock(); - } - } - - void MapNetwork::requestFinished(int id, bool error) - { - // MG::SLEEP::sleep(1); -// qDebug() << "MapNetwork::requestFinished" << http->state() << ", id: " << id; - if (error) - { - qDebug() << "network error: " << http->errorString(); - //restart query - - } - else if (vectorMutex.tryLock()) - { - // check if id is in map? - if (loadingMap.contains(id)) - { - - QString url = loadingMap[id]; - loadingMap.remove(id); - vectorMutex.unlock(); -// qDebug() << "request finished for id: " << id << ", belongs to: " << notifier.url << endl; - QByteArray ax; - - if (http->bytesAvailable()>0) - { - QPixmap pm; - ax = http->readAll(); - - if (pm.loadFromData(ax)) - { - loaded += pm.size().width()*pm.size().height()*pm.depth()/8/1024; -// qDebug() << "Network loaded: " << (loaded); - parent->receivedImage(pm, url); - } - else - { - qDebug() << "NETWORK_PIXMAP_ERROR: " << ax; - } - } - - } - else - vectorMutex.unlock(); - - } - if (loadingMap.size() == 0) - { -// qDebug () << "all loaded"; - parent->loadingQueueEmpty(); - } - } - - void MapNetwork::abortLoading() - { - http->clearPendingRequests(); -// http->abort(); - if (vectorMutex.tryLock()) - { - loadingMap.clear(); - vectorMutex.unlock(); - } - } - - bool MapNetwork::imageIsLoading(QString url) - { - return loadingMap.values().contains(url); - } - - void MapNetwork::setProxy(QString host, int port) - { -#ifndef Q_WS_QWS - http->setProxy(host, port); -#endif - } -} diff --git a/src/lib/qmapcontrol/src/mapnetwork.h b/src/lib/qmapcontrol/src/mapnetwork.h deleted file mode 100644 index fa65e937fcd22c8df8e750e01fe19489d0d4aadc..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/mapnetwork.h +++ /dev/null @@ -1,77 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef MAPNETWORK_H -#define MAPNETWORK_H - -#include -#include -#include -#include -#include -#include "imagemanager.h" -/** - @author Kai Winter - */ -namespace qmapcontrol -{ - class ImageManager; - class MapNetwork : QObject - { - Q_OBJECT - public: - MapNetwork(ImageManager* parent); - ~MapNetwork(); - - void loadImage(const QString& host, const QString& url); - - /*! - * checks if the given url is already loading - * @param url the url of the image - * @return boolean, if the image is already loading - */ - bool imageIsLoading(QString url); - - /*! - * Aborts all current loading threads. - * This is useful when changing the zoom-factor, though newly needed images loads faster - */ - void abortLoading(); - void setProxy(QString host, int port); - - private: - ImageManager* parent; - QHttp* http; - QMap loadingMap; - qreal loaded; - QMutex vectorMutex; - MapNetwork& operator=(const MapNetwork& rhs); - MapNetwork(const MapNetwork& old); - - private slots: - void requestFinished(int id, bool error); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/osmmapadapter.cpp b/src/lib/qmapcontrol/src/osmmapadapter.cpp deleted file mode 100644 index e07e3fa11578140ad1914c6c7cec272fc681eecf..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/osmmapadapter.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "osmmapadapter.h" -namespace qmapcontrol -{ - OSMMapAdapter::OSMMapAdapter() -// : TileMapAdapter("192.168.8.1", "/img/img_cache.php/%1/%2/%3.png", 256, 0, 17) - : TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17) - { - } - - OSMMapAdapter::~OSMMapAdapter() - { - } -} diff --git a/src/lib/qmapcontrol/src/osmmapadapter.h b/src/lib/qmapcontrol/src/osmmapadapter.h deleted file mode 100644 index 3807fbc716a1f69fbe73d947297aa8f006e3bffa..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/osmmapadapter.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef OSMMAPADAPTER_H -#define OSMMAPADAPTER_H - -#include "tilemapadapter.h" -namespace qmapcontrol -{ - //! MapAdapter for OpenStreetMap - /*! - * This is a conveniece class, which extends and configures a TileMapAdapter - * @author Kai Winter - */ - class OSMMapAdapter : public TileMapAdapter - { - Q_OBJECT - public: - //! constructor - /*! - * This construct a OpenStreetmap Adapter - */ - OSMMapAdapter(); - virtual ~OSMMapAdapter(); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/point.cpp b/src/lib/qmapcontrol/src/point.cpp deleted file mode 100644 index 4493d4090b94d47a0e9586240ff6dc81b94c045c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/point.cpp +++ /dev/null @@ -1,330 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "point.h" -namespace qmapcontrol -{ - Point::Point() - {} - Point::Point(const Point& point) - :Geometry(point.name()), X(point.longitude()), Y(point.latitude()) - { - visible = point.isVisible(); - mywidget = 0; - mypixmap = 0; - mypen = point.mypen; - homelevel = -1; - minsize = QSize(-1,-1); - maxsize = QSize(-1,-1); - } -//protected: - Point::Point(qreal x, qreal y, QString name, enum Alignment alignment) - : Geometry(name), X(x), Y(y), myalignment(alignment) - { - GeometryType = "Point"; - mywidget = 0; - mypixmap = 0; - visible = true; - homelevel = -1; - minsize = QSize(-1,-1); - maxsize = QSize(-1,-1); - } - - Point::Point(qreal x, qreal y, QWidget* widget, QString name, enum Alignment alignment) - : Geometry(name), X(x), Y(y), mywidget(widget), myalignment(alignment) - { -// Point(x, y, name, alignment); - GeometryType = "Point"; - mypixmap = 0; - visible = true; - size = widget->size(); - homelevel = -1; - minsize = QSize(-1,-1); - maxsize = QSize(-1,-1); - mywidget->show(); - } - Point::Point(qreal x, qreal y, QPixmap* pixmap, QString name, enum Alignment alignment) - : Geometry(name), X(x), Y(y), mypixmap(pixmap), myalignment(alignment) - { - GeometryType = "Point"; - mywidget = 0; - visible = true; - size = pixmap->size(); - homelevel = -1; - minsize = QSize(-1,-1); - maxsize = QSize(-1,-1); - } -/* - Point& Point::operator=(const Point& rhs) - { - if (this == &rhs) - return *this; - else - { - X = rhs.X; - Y = rhs.Y; - size = rhs.size; - - mywidget = rhs.mywidget; - mypixmap = rhs.mypixmap; - alignment = rhs.alignment; - homelevel = rhs.homelevel; - minsize = rhs.minsize; - maxsize = rhs.maxsize; -} -} -*/ - Point::~Point() - { - delete mywidget; - delete mypixmap; - } - - void Point::setVisible(bool visible) - { - this->visible = visible; - if (mywidget !=0) - { - mywidget->setVisible(visible); - } - } - - QRectF Point::boundingBox() - { - //TODO: have to be calculated in relation to alignment... - return QRectF(QPointF(X, Y), displaysize); - } - - qreal Point::longitude() const - { - return X; - } - qreal Point::latitude() const - { - return Y; - } - QPointF Point::coordinate() const - { - return QPointF(X, Y); - } - - void Point::draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &viewport, const QPoint offset) - { - if (!visible) - return; - - if (homelevel > 0) - { - - int currentzoom = mapadapter->maxZoom() < mapadapter->minZoom() ? mapadapter->minZoom() - mapadapter->currentZoom() : mapadapter->currentZoom(); - -// int currentzoom = mapadapter->getZoom(); - int diffzoom = homelevel-currentzoom; - int viewheight = size.height(); - int viewwidth = size.width(); - viewheight = int(viewheight / pow(2, diffzoom)); - viewwidth = int(viewwidth / pow(2, diffzoom)); - - if (minsize.height()!= -1 && viewheight < minsize.height()) - viewheight = minsize.height(); - else if (maxsize.height() != -1 && viewheight > maxsize.height()) - viewheight = maxsize.height(); - - - if (minsize.width()!= -1 && viewwidth < minsize.width()) - viewwidth = minsize.width(); - else if (maxsize.width() != -1 && viewwidth > maxsize.width()) - viewwidth = maxsize.width(); - - - displaysize = QSize(viewwidth, viewheight); - } - else - { - displaysize = size; - } - - - if (mypixmap !=0) - { - const QPointF c = QPointF(X, Y); - QPoint point = mapadapter->coordinateToDisplay(c); - - if (viewport.contains(point)) - { - QPoint alignedtopleft = alignedPoint(point); - painter->drawPixmap(alignedtopleft.x(), alignedtopleft.y(), displaysize.width(), displaysize.height(), *mypixmap); - } - - } - else if (mywidget!=0) - { - drawWidget(mapadapter, offset); - } - - } - - void Point::drawWidget(const MapAdapter* mapadapter, const QPoint offset) - { - const QPointF c = QPointF(X, Y); - QPoint point = mapadapter->coordinateToDisplay(c); - point -= offset; - - QPoint alignedtopleft = alignedPoint(point); - mywidget->setGeometry(alignedtopleft.x(), alignedtopleft.y(), displaysize.width(), displaysize.height()); - } - - QPoint Point::alignedPoint(const QPoint point) const - { - QPoint alignedtopleft; - if (myalignment == Middle) - { - alignedtopleft.setX(point.x()-displaysize.width()/2); - alignedtopleft.setY(point.y()-displaysize.height()/2); - } - else if (myalignment == TopLeft) - { - alignedtopleft.setX(point.x()); - alignedtopleft.setY(point.y()); - } - else if (myalignment == TopRight) - { - alignedtopleft.setX(point.x()-displaysize.width()); - alignedtopleft.setY(point.y()); - } - else if (myalignment == BottomLeft) - { - alignedtopleft.setX(point.x()); - alignedtopleft.setY(point.y()-displaysize.height()); - } - else if (myalignment == BottomRight) - { - alignedtopleft.setX(point.x()-displaysize.width()); - alignedtopleft.setY(point.y()-displaysize.height()); - } - return alignedtopleft; - } - - - bool Point::Touches(Point* p, const MapAdapter* mapadapter) - { - if (this->isVisible() == false) - return false; - if (mypixmap == 0) - return false; - - QPointF c = p->coordinate(); - // coordinate nach pixel umrechnen - QPoint pxOfPoint = mapadapter->coordinateToDisplay(c); - // size/2 Pixel toleranz aufaddieren - QPoint p1; - QPoint p2; - - switch (myalignment) - { - case Middle: - p1 = pxOfPoint - QPoint(displaysize.width()/2,displaysize.height()/2); - p2 = pxOfPoint + QPoint(displaysize.width()/2,displaysize.height()/2); - break; - case TopLeft: - p1 = pxOfPoint - QPoint(displaysize.width(),displaysize.height()); - p2 = pxOfPoint; - break; - case TopRight: - p1 = pxOfPoint - QPoint(0, displaysize.height()); - p2 = pxOfPoint + QPoint(displaysize.width(),0); - break; - case BottomLeft: - p1 = pxOfPoint - QPoint(displaysize.width(), 0); - p2 = pxOfPoint + QPoint(0, displaysize.height()); - break; - case BottomRight: - p1 = pxOfPoint; - p2 = pxOfPoint + QPoint(displaysize.width(), displaysize.height()); - break; - } - - // "Bounding Box" in koordinate umrechnen - QPointF c1 = mapadapter->displayToCoordinate(p1); - QPointF c2 = mapadapter->displayToCoordinate(p2); - - - if(this->longitude()>=c1.x() && this->longitude()<=c2.x()) - { - if (this->latitude()<=c1.y() && this->latitude()>=c2.y()) - { - emit(geometryClicked(this, QPoint(0,0))); - return true; - } - } - return false; - } - - void Point::setCoordinate(QPointF point) - { -// emit(updateRequest(this)); -// emit(updateRequest(QRectF(X, Y, size.width(), size.height()))); - X = point.x(); - Y = point.y(); -// emit(updateRequest(this)); - emit(updateRequest(QRectF(X, Y, size.width(), size.height()))); - - emit(positionChanged(this)); - } - QList Point::points() - { - //TODO: assigning temp?! - QList points; - points.append(this); - return points; - } - - QWidget* Point::widget() - { - return mywidget; - } - - QPixmap* Point::pixmap() - { - return mypixmap; - } - - void Point::setBaselevel(int zoomlevel) - { - homelevel = zoomlevel; - } - void Point::setMinsize(QSize minsize) - { - this->minsize = minsize; - } - void Point::setMaxsize(QSize maxsize) - { - this->maxsize = maxsize; - } - Point::Alignment Point::alignment() const - { - return myalignment; - } -} diff --git a/src/lib/qmapcontrol/src/point.h b/src/lib/qmapcontrol/src/point.h deleted file mode 100644 index b9e0de0b3665cd02eb56d0c7ab278992bac3b53b..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/point.h +++ /dev/null @@ -1,214 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef POINT_H -#define POINT_H -#include - -#include "geometry.h" - -namespace qmapcontrol -{ - //! A geometric point to draw objects into maps - /*! - * This class can be used to draw your custom QPixmap or other QWidgets into maps. - * You can instantiate a Point with any Pixmap you want. The objects cares about collision detection (for clickable objects) - * - * When drawing a pixmap, take care you are adding the point to a GeometryLayer. - * You can also add a point to a MapLayer, but this should only be done, if the point is not changing its position or color etc. - * (GeometryLayers are assured to be repainted on any changes at the point. MapLayers only gets repainted, if a new - * offscreenImage is painter. This is a performance issue.) - * - * Points emit click events, if the containing layer receives clickevents (the default) - * - * You can also add a widget into maps. But keep in mind, that widgets always are drawn on top of all layers. - * You also have to handle click events yourself. - * - * To create "zoomable objects" (objects that increases size on zooming), a base level have to be set. - * The base level is the zoom level on which the point´s pixmap gets displayed on full size. - * On lower zoom levels it gets displayed smaller and on higher zoom levels larger. - * A minimal size can be set as well as a maximum size. - * @see setBaselevel, setMinsize, setMaxsize - * - * @author Kai Winter - */ - class Point : public Geometry - { - Q_OBJECT - public: - friend class Layer; - friend class LineString; - - //! sets where the point should be aligned - enum Alignment - { - TopLeft, /*!< Align on TopLeft*/ - TopRight, /*!< Align on TopRight*/ - BottomLeft, /*!< Align on BottomLeft*/ - BottomRight,/*!< Align on BottomRight*/ - Middle /*!< Align on Middle*/ - }; - - Point(); - explicit Point(const Point&); - //! Copy Constructor - /*! - * This constructor creates a Point with no image or widget. - * @param x longitude - * @param y latitude - * @param name name of the point - * @param alignment allignment of the point (Middle or TopLeft) - */ - Point(qreal x, qreal y, QString name = QString(), enum Alignment alignment=Middle); - - //! Constructor - /*! - * This constructor creates a point which will display the given widget. - * You can set an alignment on which corner the widget should be aligned to the coordinate. - * You have to set the size of the widget, before adding it to - * IMPORTANT: You have to set the QMapControl as parent for the widget! - * @param x longitude - * @param y latitude - * @param widget the widget which should be displayed by this point - * @param name name of the point - * @param alignment allignment of the point (Middle or TopLeft) - */ - Point(qreal x, qreal y, QWidget* widget, QString name = QString(), enum Alignment alignment = Middle); - - //! Constructor - /*! - * This constructor creates a point which will display the give pixmap. - * You can set an alignment on which corner the pixmap should be aligned to the coordinate. - * @param x longitude - * @param y latitude - * @param pixmap the pixmap which should be displayed by this point - * @param name name of the point - * @param alignment allignment of the point (Middle or TopLeft) - */ - Point(qreal x, qreal y, QPixmap* pixmap, QString name = QString(), enum Alignment alignment = Middle); - virtual ~Point(); - - //! returns the bounding box of the point - /*! - * The Bounding contains the coordinate of the point and its size. - * The size is set, if the point contains a pixmap or a widget - * @return the bounding box of the point - */ - virtual QRectF boundingBox(); - - //! returns the longitude of the point - /*! - * @return the longitude of the point - */ - qreal longitude() const; - - //! returns the latitude of the point - /*! - * @return the latitude of the point - */ - qreal latitude() const; - - //! returns the coordinate of the point - /*! - * The x component of the returned QPointF is the longitude value, - * the y component the latitude - * @return the coordinate of a point - */ - QPointF coordinate() const; - - virtual QList points(); - - /*! \brief returns the widget of the point - @return the widget of the point - */ - QWidget* widget(); - - //! returns the pixmap of the point - /*! - * @return the pixmap of the point - */ - QPixmap* pixmap(); - - //! Sets the zoom level on which the point´s pixmap gets displayed on full size - /*! - * Use this method to set a zoom level on which the pixmap gets displayed with its real size. - * On zoomlevels below it will be displayed smaller, and on zoom levels thereover it will be displayed larger - * @see setMinsize, setMaxsize - * @param zoomlevel the zoomlevel on which the point will be displayed on full size - */ - void setBaselevel(int zoomlevel); - - //! sets a minimal size for the pixmap - /*! - * When the point´s pixmap should change its size on zooming, this method sets the minimal size. - * @see setBaselevel - * @param minsize the minimal size which the pixmap should have - */ - void setMinsize(QSize minsize); - - //! sets a maximal size for the pixmap - /*! - * When the point´s pixmap should change its size on zooming, this method sets the maximal size. - * @see setBaselevel - * @param maxsize the maximal size which the pixmap should have - */ - void setMaxsize(QSize maxsize); - - Point::Alignment alignment() const; - - protected: - qreal X; - qreal Y; - QSize size; - - QWidget* mywidget; - QPixmap* mypixmap; - Alignment myalignment; - int homelevel; - QSize displaysize; - QSize minsize; - QSize maxsize; - - - void drawWidget(const MapAdapter* mapadapter, const QPoint offset); -// void drawPixmap(QPainter* painter, const MapAdapter* mapadapter, const QRect &viewport, const QPoint versch); - virtual void draw(QPainter* painter, const MapAdapter* mapadapter, const QRect &viewport, const QPoint offset); - QPoint alignedPoint(const QPoint point) const; - - //! returns true if the given Point touches this Point - /*! - * The collision detection checks for the bounding rectangulars. - * @param geom the other point which should be tested on collision - * @param mapadapter the mapadapter which is used for calculations - * @return - */ - virtual bool Touches(Point* geom, const MapAdapter* mapadapter); - - public slots: - void setCoordinate(QPointF point); - virtual void setVisible(bool visible); - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/qmapcontrol.pri b/src/lib/qmapcontrol/src/qmapcontrol.pri deleted file mode 100644 index adbe041dc67a82e4cffe13ccff7aea5b8253825b..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/qmapcontrol.pri +++ /dev/null @@ -1,49 +0,0 @@ -#DEPENDPATH += src -#INCLUDEPATH += src -QMAPCONTROLDIR = src/lib/qmapcontrol - -# Input -HEADERS += curve.h \ - geometry.h \ - imagemanager.h \ - layer.h \ - layermanager.h \ - linestring.h \ - mapadapter.h \ - mapcontrol.h \ - mapnetwork.h \ - point.h \ - tilemapadapter.h \ - wmsmapadapter.h \ - circlepoint.h \ - imagepoint.h \ - gps_position.h \ - osmmapadapter.h \ - maplayer.h \ - geometrylayer.h \ - yahoomapadapter.h \ - googlemapadapter.h \ - googlesatmapadapter.h -SOURCES += curve.cpp \ - geometry.cpp \ - imagemanager.cpp \ - layer.cpp \ - layermanager.cpp \ - linestring.cpp \ - mapadapter.cpp \ - mapcontrol.cpp \ - mapnetwork.cpp \ - point.cpp \ - tilemapadapter.cpp \ - wmsmapadapter.cpp \ - circlepoint.cpp \ - imagepoint.cpp \ - gps_position.cpp \ - osmmapadapter.cpp \ - maplayer.cpp \ - geometrylayer.cpp \ - yahoomapadapter.cpp \ - googlemapadapter.cpp \ - googlesatmapadapter.cpp - -QT += network diff --git a/src/lib/qmapcontrol/src/tilemapadapter.cpp b/src/lib/qmapcontrol/src/tilemapadapter.cpp deleted file mode 100644 index fa0102920c0fe462fb1efa1daa6af8e31206a2a9..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/tilemapadapter.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "tilemapadapter.h" -namespace qmapcontrol -{ - TileMapAdapter::TileMapAdapter(const QString& host, const QString& serverPath, int tilesize, int minZoom, int maxZoom) - :MapAdapter(host, serverPath, tilesize, minZoom, maxZoom) - { - PI = acos(-1.0); - - /* - Initialize the "substring replace engine". First the string replacement - in getQuery was made by QString().arg() but this was very slow. So this - splits the servers path into substrings and when calling getQuery the - substrings get merged with the parameters of the URL. - Pretty complicated, but fast. - */ - param1 = serverPath.indexOf("%1"); - param2 = serverPath.indexOf("%2"); - param3 = serverPath.indexOf("%3"); - - int min = param1 < param2 ? param1 : param2; - min = param3 < min ? param3 : min; - - int max = param1 > param2 ? param1 : param2; - max = param3 > max ? param3 : max; - - int middle = param1+param2+param3-min-max; - - order[0][0] = min; - if (min == param1) - order[0][1] = 0; - else if (min == param2) - order[0][1] = 1; - else - order[0][1] = 2; - - order[1][0] = middle; - if (middle == param1) - order[1][1] = 0; - else if (middle == param2) - order[1][1] = 1; - else - order[1][1] = 2; - - order[2][0] = max; - if (max == param1) - order[2][1] = 0; - else if(max == param2) - order[2][1] = 1; - else - order[2][1] = 2; - - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - numberOfTiles = tilesonzoomlevel(zoom); - loc.setNumberOptions(QLocale::OmitGroupSeparator); - } - - TileMapAdapter::~TileMapAdapter() - { - } - //TODO: pull out - void TileMapAdapter::zoom_in() - { - if (min_zoom > max_zoom) - { - //current_zoom = current_zoom-1; - current_zoom = current_zoom > max_zoom ? current_zoom-1 : max_zoom; - } - else if (min_zoom < max_zoom) - { - //current_zoom = current_zoom+1; - current_zoom = current_zoom < max_zoom ? current_zoom+1 : max_zoom; - } - - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - numberOfTiles = tilesonzoomlevel(zoom); - - } - void TileMapAdapter::zoom_out() - { - if (min_zoom > max_zoom) - { - //current_zoom = current_zoom+1; - current_zoom = current_zoom < min_zoom ? current_zoom+1 : min_zoom; - } - else if (min_zoom < max_zoom) - { - //current_zoom = current_zoom-1; - current_zoom = current_zoom > min_zoom ? current_zoom-1 : min_zoom; - } - - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - numberOfTiles = tilesonzoomlevel(zoom); - } - - qreal TileMapAdapter::deg_rad(qreal x) const - { - return x * (PI/180.0); - } - qreal TileMapAdapter::rad_deg(qreal x) const - { - return x * (180/PI); - } - - QString TileMapAdapter::query(int x, int y, int z) const - { - x = xoffset(x); - y = yoffset(y); - - int a[3] = {z, x, y}; - return QString(serverPath).replace(order[2][0],2, loc.toString(a[order[2][1]])) - .replace(order[1][0],2, loc.toString(a[order[1][1]])) - .replace(order[0][0],2, loc.toString(a[order[0][1]])); - - } - - QPoint TileMapAdapter::coordinateToDisplay(const QPointF& coordinate) const - { - qreal x = (coordinate.x()+180) * (numberOfTiles*mytilesize)/360.; // coord to pixel! - qreal y = (1-(log(tan(PI/4+deg_rad(coordinate.y())/2)) /PI)) /2 * (numberOfTiles*mytilesize); - - return QPoint(int(x), int(y)); - } - - QPointF TileMapAdapter::displayToCoordinate(const QPoint& point) const - { - qreal longitude = (point.x()*(360/(numberOfTiles*mytilesize)))-180; - qreal latitude = rad_deg(atan(sinh((1-point.y()*(2/(numberOfTiles*mytilesize)))*PI))); - - return QPointF(longitude, latitude); - - } - - bool TileMapAdapter::isValid(int x, int y, int z) const - { - if (max_zoom < min_zoom) - { - z= min_zoom - z; - } - - if (x<0 || x>pow(2,z)-1 || - y<0 || y>pow(2,z)-1) - { - return false; - } - return true; - - } - int TileMapAdapter::tilesonzoomlevel(int zoomlevel) const - { - return int(pow(2, zoomlevel)); - } - int TileMapAdapter::xoffset(int x) const - { - return x; - } - int TileMapAdapter::yoffset(int y) const - { - return y; - } -} diff --git a/src/lib/qmapcontrol/src/tilemapadapter.h b/src/lib/qmapcontrol/src/tilemapadapter.h deleted file mode 100644 index 9b1726363a836dfb60a31185e384bb7adf8d1d0a..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/tilemapadapter.h +++ /dev/null @@ -1,76 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef TILEMAPADAPTER_H -#define TILEMAPADAPTER_H - -#include "mapadapter.h" - -namespace qmapcontrol -{ - //! MapAdapter for servers with image tiles - /*! - * Use this derived MapAdapter to display maps from OpenStreetMap - * @author Kai Winter - */ - class TileMapAdapter : public MapAdapter - { - Q_OBJECT - public: - //! constructor - /*! - * Sample of a correct initialization of a MapAdapter:
- * TileMapAdapter* ta = new TileMapAdapter("192.168.8.1", "/img/img_cache.php/%1/%2/%3.png", 256, 0,17);
- * The placeholders %1, %2, %3 stands for x, y, z
- * The minZoom is 0 (means the whole world is visible). The maxZoom is 17 (means it is zoomed in to the max) - * @param host The servers URL - * @param serverPath The path to the tiles with placeholders - * @param tilesize the size of the tiles - * @param minZoom the minimum zoom level - * @param maxZoom the maximum zoom level - */ - TileMapAdapter(const QString& host, const QString& serverPath, int tilesize, int minZoom = 0, int maxZoom = 17); - - virtual ~TileMapAdapter(); - - virtual QPoint coordinateToDisplay(const QPointF&) const; - virtual QPointF displayToCoordinate(const QPoint&) const; - - qreal PI; - - protected: - qreal rad_deg(qreal) const; - qreal deg_rad(qreal) const; - - virtual bool isValid(int x, int y, int z) const; - virtual void zoom_in(); - virtual void zoom_out(); - virtual QString query(int x, int y, int z) const; - virtual int tilesonzoomlevel(int zoomlevel) const; - virtual int xoffset(int x) const; - virtual int yoffset(int y) const; - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/wmsmapadapter.cpp b/src/lib/qmapcontrol/src/wmsmapadapter.cpp deleted file mode 100644 index 085ff504d21286be9172cacc12ed5a4036d9ed9c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/wmsmapadapter.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "wmsmapadapter.h" -namespace qmapcontrol -{ - WMSMapAdapter::WMSMapAdapter(QString host, QString serverPath, int tilesize) - : MapAdapter(host, serverPath, tilesize, 0, 17) - { -// param1 = serverPath.indexOf("%1"); -// param2 = serverPath.indexOf("%2"); -// param3 = serverPath.indexOf("%3"); -// param4 = serverPath.indexOf("%4"); -// param5 = serverPath.indexOf("%5"); -// param6 = serverPath.lastIndexOf("%5"); - -// this->serverPath = serverPath.replace(param6, 2, QString().setNum(tilesize)).replace(param5, 2, QString().setNum(tilesize)); - -// sub1 = serverPath.mid(0, param1); -// sub2 = serverPath.mid(param1+2, param2-param1-2); -// sub3 = serverPath.mid(param2+2, param3-param2-2); -// sub4 = serverPath.mid(param3+2, param4-param3-2); -// sub5 = serverPath.mid(param4+2); - - this->serverPath.append("&WIDTH=").append(loc.toString(tilesize)) - .append("&HEIGHT=").append(loc.toString(tilesize)) - .append("&BBOX="); - numberOfTiles = pow(2, current_zoom); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; - } - - - WMSMapAdapter::~WMSMapAdapter() - { - } - - QPoint WMSMapAdapter::coordinateToDisplay(const QPointF& coordinate) const - { - qreal x = (coordinate.x()+180) * (numberOfTiles*mytilesize)/360.; // coord to pixel! - qreal y = -1*(coordinate.y()-90) * (numberOfTiles*mytilesize)/180.; // coord to pixel! - return QPoint(int(x), int(y)); - } - QPointF WMSMapAdapter::displayToCoordinate(const QPoint& point) const - { - qreal lon = (point.x()*(360./(numberOfTiles*mytilesize)))-180; - qreal lat = -(point.y()*(180./(numberOfTiles*mytilesize)))+90; - return QPointF(lon, lat); - } - void WMSMapAdapter::zoom_in() - { - current_zoom+=1; - numberOfTiles = pow(2, current_zoom); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; - } - void WMSMapAdapter::zoom_out() - { - current_zoom-=1; - numberOfTiles = pow(2, current_zoom); - coord_per_x_tile = 360. / numberOfTiles; - coord_per_y_tile = 180. / numberOfTiles; - } - - bool WMSMapAdapter::isValid(int /*x*/, int /*y*/, int /*z*/) const - { -// if (x>0 && y>0 && z>0) - { - return true; - } -// return false; - } - QString WMSMapAdapter::query(int i, int j, int /*z*/) const - { - return getQ(-180+i*coord_per_x_tile, - 90-(j+1)*coord_per_y_tile, - -180+i*coord_per_x_tile+coord_per_x_tile, - 90-(j+1)*coord_per_y_tile+coord_per_y_tile); - } - QString WMSMapAdapter::getQ(qreal ux, qreal uy, qreal ox, qreal oy) const - { - return QString().append(serverPath) - .append(loc.toString(ux)).append(",") - .append(loc.toString(uy)).append(",") - .append(loc.toString(ox)).append(",") - .append(loc.toString(oy)); - } -} diff --git a/src/lib/qmapcontrol/src/wmsmapadapter.h b/src/lib/qmapcontrol/src/wmsmapadapter.h deleted file mode 100644 index 3324380f887e8c35b7dbdaba46d2a22c3151d36c..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/wmsmapadapter.h +++ /dev/null @@ -1,71 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef WMSMAPADAPTER_H -#define WMSMAPADAPTER_H - -#include "mapadapter.h" - -namespace qmapcontrol -{ - //! MapAdapter for WMS servers - /*! - * Use this derived MapAdapter to display maps from WMS servers - * @author Kai Winter - */ - class WMSMapAdapter : public MapAdapter - { - public: - //! constructor - /*! - * Sample of a correct initialization of a MapAdapter:
- * MapAdapter* mapadapter = new WMSMapAdapter("www2.demis.nl", "/wms/wms.asp?wms=WorldMap[...]&BBOX=%1,%2,%3,%4&WIDTH=%5&HEIGHT=%5&TRANSPARENT=TRUE", 256);
- * The placeholders %1, %2, %3, %4 creates the bounding box, %5 is for the tilesize - * The minZoom is 0 (means the whole world is visible). The maxZoom is 17 (means it is zoomed in to the max) - * @param host The servers URL - * @param serverPath The path to the tiles with placeholders - * @param tilesize the size of the tiles - */ - WMSMapAdapter(QString host, QString serverPath, int tilesize = 256); - virtual ~WMSMapAdapter(); - - virtual QPoint coordinateToDisplay(const QPointF&) const; - virtual QPointF displayToCoordinate(const QPoint&) const; - - - protected: - virtual void zoom_in(); - virtual void zoom_out(); - virtual QString query(int x, int y, int z) const; - virtual bool isValid(int x, int y, int z) const; - - private: - virtual QString getQ(qreal ux, qreal uy, qreal ox, qreal oy) const; - - qreal coord_per_x_tile; - qreal coord_per_y_tile; - }; -} -#endif diff --git a/src/lib/qmapcontrol/src/yahoomapadapter.cpp b/src/lib/qmapcontrol/src/yahoomapadapter.cpp deleted file mode 100644 index 55f8688e289d16d771fde484fbd6e5b643458783..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/yahoomapadapter.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#include "yahoomapadapter.h" -namespace qmapcontrol -{ - YahooMapAdapter::YahooMapAdapter() - : TileMapAdapter("png.maps.yimg.com", "/png?v=3.1.0&x=%2&y=%3&z=%1", 256, 17,0) - { - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - numberOfTiles = pow(2, zoom+1); - } - YahooMapAdapter::YahooMapAdapter(QString host, QString url) - : TileMapAdapter(host, url, 256, 17,0) - { - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - numberOfTiles = pow(2, zoom+1); - } - YahooMapAdapter::~YahooMapAdapter() - { - } - - bool YahooMapAdapter::isValid(int /*x*/, int /*y*/, int /*z*/) const - { - return true; - } - - int YahooMapAdapter::tilesonzoomlevel(int zoomlevel) const - { - return int(pow(2, zoomlevel+1)); - } - - int YahooMapAdapter::yoffset(int y) const - { - int zoom = max_zoom < min_zoom ? min_zoom - current_zoom : current_zoom; - - int tiles = int(pow(2, zoom)); - y = y*(-1)+tiles-1; - return int(y); - } -} diff --git a/src/lib/qmapcontrol/src/yahoomapadapter.h b/src/lib/qmapcontrol/src/yahoomapadapter.h deleted file mode 100644 index 8cd0b50eaea3a43256eee6bfd6fe64c60165e578..0000000000000000000000000000000000000000 --- a/src/lib/qmapcontrol/src/yahoomapadapter.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -* -* This file is part of QMapControl, -* an open-source cross-platform map widget -* -* Copyright (C) 2007 - 2008 Kai Winter -* -* This program is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* This program 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 Lesser General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with QMapControl. If not, see . -* -* Contact e-mail: kaiwinter@gmx.de -* Program URL : http://qmapcontrol.sourceforge.net/ -* -*/ - -#ifndef YAHOOMAPADAPTER_H -#define YAHOOMAPADAPTER_H - -#include "tilemapadapter.h" - -namespace qmapcontrol -{ - //! MapAdapter for Yahoo Maps - /*! - * @author Kai Winter - */ - class YahooMapAdapter : public TileMapAdapter - { - Q_OBJECT - public: - //! constructor - /*! - * This construct a Yahoo Adapter - */ - YahooMapAdapter(); - YahooMapAdapter(QString host, QString url); - virtual ~YahooMapAdapter(); - bool isValid(int x, int y, int z) const; - protected: - virtual int tilesonzoomlevel(int zoomlevel) const; - virtual int yoffset(int y) const; - }; -} -#endif diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 873a6d98e09165a926ef8a3d6d0bacd96201108e..6f79c3b84d613cf676bb5a1de1f405bb0012e5f1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -81,11 +81,13 @@ roll(0.0), pitch(0.0), yaw(0.0), statusTimeout(new QTimer(this)), -paramsOnceRequested(false) +paramsOnceRequested(false), +airframe(0) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); + connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout->start(500); readSettings(); } @@ -102,6 +104,8 @@ void UAS::writeSettings() QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); settings.setValue("NAME", this->name); + settings.setValue("AIRFRAME", this->airframe); + settings.setValue("AP_TYPE", this->autopilot); settings.endGroup(); settings.sync(); } @@ -111,6 +115,8 @@ void UAS::readSettings() QSettings settings; settings.beginGroup(QString("MAV%1").arg(uasId)); this->name = settings.value("NAME", this->name).toString(); + this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); + this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); settings.endGroup(); } @@ -146,18 +152,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 +200,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: @@ -190,6 +213,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (this->type != mavlink_msg_heartbeat_get_type(&message)) { this->type = mavlink_msg_heartbeat_get_type(&message); + if (airframe == 0) + { + switch (type) + { + case MAV_FIXED_WING: + setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); + break; + case MAV_QUADROTOR: + setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); + break; + default: + // Do nothing + break; + } + } this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); emit systemTypeSet(this, type); } @@ -326,15 +364,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: @@ -347,6 +385,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) roll = attitude.roll; pitch = attitude.pitch; yaw = attitude.yaw; + + roll = QGC::limitAngleToPMPI(roll); + pitch = QGC::limitAngleToPMPI(pitch); + yaw = QGC::limitAngleToPMPI(yaw); + // emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); // emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); // emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); @@ -1038,7 +1081,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"); @@ -1374,6 +1417,7 @@ void UAS::setUASName(const QString& name) this->name = name; writeSettings(); emit nameChanged(name); + emit systemSpecsChanged(uasId); } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index dfd9d11d4693a7e50fb41fbd2435dde1527e8725..d65d319f00aa942e83f870e9d064983c22fab823 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -70,6 +70,8 @@ public: QString getUASName(void) const; /** @brief Get the unique system id */ int getUASID() const; + /** @brief Get the airframe */ + int getAirframe() const { return airframe; } /** @brief The time interval the robot is switched on */ quint64 getUptime() const; /** @brief Get the status flag for the communication */ @@ -89,9 +91,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) @@ -156,6 +159,7 @@ protected: //COMMENTS FOR TEST UNIT QTimer* statusTimeout; ///< Timer for various status timeouts QMap* > parameters; ///< All parameters bool paramsOnceRequested; ///< If the parameter list has been read at least once + int airframe; ///< The airframe type public: /** @brief Set the current battery type */ void setBattery(BatteryType type, int cells); @@ -175,9 +179,11 @@ public: public slots: /** @brief Set the autopilot type */ - void setAutopilotType(int apType) { autopilot = apType; } + void setAutopilotType(int apType) { autopilot = apType; emit systemSpecsChanged(uasId); } /** @brief Set the type of airframe */ - void setSystemType(int systemType) { type = systemType; } + void setSystemType(int systemType) { type = systemType; emit systemSpecsChanged(uasId); } + /** @brief Set the specific airframe type */ + void setAirframe(int airframe) { this->airframe = airframe; emit systemSpecsChanged(uasId); } /** @brief Set a new name **/ void setUASName(const QString& name); /** @brief Executes an action **/ @@ -308,6 +314,8 @@ signals: protected: /** @brief Get the UNIX timestamp in microseconds */ quint64 getUnixTime(quint64 time); + +protected slots: /** @brief Write settings to disk */ void writeSettings(); /** @brief Read settings from disk */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 514d596a24d7cdd6ced9da27aede6ed239186a55..cf7f65b4891e69ebf0cedb5a359510bdd9129126 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -78,6 +78,11 @@ public: virtual double getPitch() const = 0; virtual double getYaw() const = 0; + virtual bool getSelected() const = 0; + + /** @brief Set the airframe of this MAV */ + virtual int getAirframe() const = 0; + /** @brief Get reference to the waypoint manager **/ virtual UASWaypointManager* getWaypointManager(void) = 0; @@ -96,7 +101,18 @@ public: COMM_TIMEDOUT = 5///< Communication link failed }; - + enum Airframe { + QGC_AIRFRAME_GENERIC = 0, + QGC_AIRFRAME_EASYSTAR, + QGC_AIRFRAME_TWINSTAR, + QGC_AIRFRAME_MERLIN, + QGC_AIRFRAME_CHEETAH, + QGC_AIRFRAME_MIKROKOPTER, + QGC_AIRFRAME_REAPER, + QGC_AIRFRAME_PREDATOR, + QGC_AIRFRAME_COAXIAL, + QGC_AIRFRAME_PTERYX + }; /** * @brief Get the links associated with this robot @@ -168,6 +184,9 @@ public slots: /** @brief Sets an action **/ virtual void setAction(MAV_ACTION action) = 0; + /** @brief Selects the airframe */ + virtual void setAirframe(int airframe) = 0; + /** @brief Launches the system/Liftof **/ virtual void launch() = 0; /** @brief Set a new waypoint **/ @@ -419,6 +438,10 @@ 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); + /** @brief Core specifications have changed */ + void systemSpecsChanged(int uasId); protected: diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 68ef921ea2f0eb79b3b4df6927baf5f6c3d15327..d7539a9cf864cd58581faa2d919cfe9232e05f85 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -69,10 +69,11 @@ UASManager::~UASManager() void UASManager::run() { - forever - { - QGC::SLEEP::msleep(5000); - } +// forever +// { +// QGC::SLEEP::msleep(5000); +// } + exec(); } void UASManager::addUAS(UASInterface* uas) @@ -93,6 +94,7 @@ void UASManager::addUAS(UASInterface* uas) if (!systems.contains(uas)) { systems.append(uas); + connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(removeUAS(QObject*))); emit UASCreated(uas); } @@ -103,6 +105,43 @@ void UASManager::addUAS(UASInterface* uas) } } +void UASManager::removeUAS(QObject* uas) +{ + UASInterface* mav = qobject_cast(uas); + + if (mav) + { + int listindex = systems.indexOf(mav); + + if (mav == activeUAS) + { + if (systems.count() > 1) + { + // We only set a new UAS if more than one is present + if (listindex != 0) + { + // The system to be removed is not at position 1 + // set position one as new active system + setActiveUAS(systems.first()); + } + else + { + // The system to be removed is at position 1, + // select the next system + setActiveUAS(systems.at(1)); + } + } + else + { + // TODO send a null pointer if no UAS is present any more + // This has to be proberly tested however, since it might + // crash code parts not handling null pointers correctly. + } + } + systems.removeAt(listindex); + } +} + QList UASManager::getUASList() { return systems; @@ -203,6 +242,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/UASManager.h b/src/uas/UASManager.h index 00441ee7089ad402b3c8c7649200c5e34acf1e54..bbdc87c4f703b0dd68a895dc16105ef0eebdb3ee 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -82,6 +82,9 @@ public slots: **/ void addUAS(UASInterface* UAS); + /** @brief Remove a system from the list */ + void removeUAS(QObject* uas); + /** * @brief Set a UAS as currently selected 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/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 15a66edd51406c98b14d4af2d71e97b546568ddf..8b0bfd8f0845bdd975944dac575b86a00ecb7878 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -63,6 +63,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn ui.linkType->addItem("Serial Forwarding",QGC_LINK_FORWARDING); ui.connectionType->addItem("MAVLink", QGC_PROTOCOL_MAVLINK); + ui.connectionType->addItem("GPS NMEA", QGC_PROTOCOL_NMEA); // Create action to open this menu // Create configuration action for this link @@ -142,6 +143,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn qDebug() << "Link is NOT a known link, can't open configuration window"; } + // Open details pane for MAVLink if necessary MAVLinkProtocol* mavlink = dynamic_cast(protocol); if (mavlink != 0) @@ -180,6 +182,11 @@ void CommConfigurationWindow::setLinkType(int linktype) Q_UNUSED(linktype); } +void CommConfigurationWindow::setProtocol(int protocol) +{ + qDebug() << "Changing to protocol" << protocol; +} + void CommConfigurationWindow::setConnection() { if(!link->isConnected()) diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index 93f1566aeceb26a15d5f080291402eaa40f470bc..27b1ad2fb5fdb1b6ed7af153acb679553c4471d3 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -49,9 +49,11 @@ enum qgc_link_t enum qgc_protocol_t { - QGC_PROTOCOL_MAVLINK + QGC_PROTOCOL_MAVLINK, + QGC_PROTOCOL_NMEA }; + #ifdef OPAL_RT #include "OpalLink.h" #endif @@ -71,6 +73,8 @@ public: public slots: void setLinkType(int linktype); + /** @brief Set the protocol for this link */ + void setProtocol(int protocol); void setConnection(); void connectionState(bool connect); void setLinkName(QString name); diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index 9bf157a16d005a9885cbbb2253a7bf5479eb8a53..82989b19cef3ec8b2114c21873a030be3a9505f8 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -29,6 +29,7 @@ This file is part of the QGROUNDCONTROL project * */ #include +#include #include "DebugConsole.h" #include "ui_DebugConsole.h" @@ -45,6 +46,7 @@ DebugConsole::DebugConsole(QWidget *parent) : holdOn(false), convertToAscii(true), filterMAVLINK(false), + autoHold(true), bytesToIgnore(0), lastByte(-1), sentBytes(), @@ -57,13 +59,15 @@ DebugConsole::DebugConsole(QWidget *parent) : dataRate(0.0f), lowpassDataRate(0.0f), dataRateThreshold(500), - autoHold(true), + commandIndex(0), m_ui(new Ui::DebugConsole) { // Setup basic user interface m_ui->setupUi(this); // Hide sent text field - it is only useful after send has been hit m_ui->sentText->setVisible(false); + // Hide auto-send checkbox + m_ui->specialCheckBox->setVisible(false); // Make text area not editable m_ui->receiveText->setReadOnly(true); // Limit to 500 lines @@ -108,16 +112,67 @@ DebugConsole::DebugConsole(QWidget *parent) : // Connect connect button connect(m_ui->connectButton, SIGNAL(clicked()), this, SLOT(handleConnectButton())); // Connect the special chars combo box - connect(m_ui->specialComboBox, SIGNAL(activated(QString)), this, SLOT(appendSpecialSymbol(QString))); + connect(m_ui->addSymbolButton, SIGNAL(clicked()), this, SLOT(appendSpecialSymbol())); + // Connect Checkbox + connect(m_ui->specialComboBox, SIGNAL(highlighted(QString)), this, SLOT(specialSymbolSelected(QString))); + // Set add button invisible if auto add checkbox is checked + connect(m_ui->specialCheckBox, SIGNAL(clicked(bool)), m_ui->addSymbolButton, SLOT(setHidden(bool))); + // Allow to send via return + connect(m_ui->sendText, SIGNAL(returnPressed()), this, SLOT(sendBytes())); + + hold(false); + + loadSettings(); - this->setVisible(false); + // Warn user about not activated hold + if (!m_ui->holdCheckBox->isChecked()) + { + m_ui->receiveText->appendHtml(QString("%2\n").arg(QColor(Qt::red).name(), tr("WARNING: You have NOT enabled auto-hold (stops updating the console is huge amounts of serial data arrive). Updating the console consumes significant CPU load, so if you receive more than about 5 KB/s of serial data, make sure to enable auto-hold if not using the console."))); + } } DebugConsole::~DebugConsole() { + storeSettings(); delete m_ui; } +void DebugConsole::loadSettings() +{ + // Load defaults from settings + QSettings settings; + settings.sync(); + settings.beginGroup("QGC_DEBUG_CONSOLE"); + m_ui->specialComboBox->setCurrentIndex(settings.value("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex()).toInt()); + m_ui->specialCheckBox->setChecked(settings.value("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked()).toBool()); + hexModeEnabled(settings.value("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked()).toBool()); + MAVLINKfilterEnabled(settings.value("MAVLINK_FILTER_ENABLED", m_ui->mavlinkCheckBox->isChecked()).toBool()); + setAutoHold(settings.value("AUTO_HOLD_ENABLED", m_ui->holdCheckBox->isChecked()).toBool()); + settings.endGroup(); + + // Update visibility settings + if (m_ui->specialCheckBox->isChecked()) + { + m_ui->specialCheckBox->setVisible(true); + m_ui->addSymbolButton->setVisible(false); + } +} + +void DebugConsole::storeSettings() +{ + // Store settings + QSettings settings; + settings.beginGroup("QGC_DEBUG_CONSOLE"); + settings.setValue("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex()); + settings.setValue("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked()); + settings.setValue("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked()); + settings.setValue("MAVLINK_FILTER_ENABLED", m_ui->mavlinkCheckBox->isChecked()); + settings.setValue("AUTO_HOLD_ENABLED", m_ui->holdCheckBox->isChecked()); + settings.endGroup(); + settings.sync(); + //qDebug() << "Storing settings!"; +} + /** * Add a link to the debug console output */ @@ -197,7 +252,8 @@ void DebugConsole::setAutoHold(bool hold) void DebugConsole::receiveTextMessage(int id, int component, int severity, QString text) { Q_UNUSED(severity); - m_ui->receiveText->appendHtml(QString("(MAV%2:%3) %4").arg(UASManager::instance()->getUASForId(id)->getColor().name(), QString::number(id), QString::number(component), text)); + m_ui->receiveText->appendHtml(QString("(MAV%2:%3) %4\n").arg(UASManager::instance()->getUASForId(id)->getColor().name(), QString::number(id), QString::number(component), text)); + //m_ui->receiveText->appendPlainText(""); } void DebugConsole::updateTrafficMeasurements() @@ -261,14 +317,14 @@ void DebugConsole::paintEvent(QPaintEvent *event) void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) { snapShotBytes += bytes.size(); - // Only add date from current link + // Only add data from current link if (link == currLink && !holdOn) { // Parse all bytes for (int j = 0; j < bytes.size(); j++) { unsigned char byte = bytes.at(j); - // Filter MAVLink (http://pixhawk.ethz.ch/mavlink) messages out of the stream. + // Filter MAVLink (http://pixhawk.ethz.ch/wiki/mavlink/) messages out of the stream. if (filterMAVLINK && bytes.size() > 1) { // Filtering is done by setting an ignore counter based on the MAVLINK packet length @@ -285,14 +341,23 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) { switch (byte) { - // Catch line feed + // Accept line feed and tab case (unsigned char)'\n': - m_ui->receiveText->appendPlainText(str); - str = ""; + { + if (lastByte != '\r') + { + // Do not break line again for CR+LF + // only break line for single LF bytes + str.append(byte); + } + } break; - // Catch carriage return + case (unsigned char)'\t': + str.append(byte); + break; + // Catch and ignore carriage return case (unsigned char)'\r': - // Ignore + str.append(byte); break; default: str.append(QChar(QChar::ReplacementCharacter)); @@ -312,6 +377,7 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) str.append(str2); } lineBuffer.append(str); + lastByte = byte; } else { @@ -321,9 +387,13 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) } } - m_ui->receiveText->appendPlainText(lineBuffer); - lineBuffer.clear(); - + if (lineBuffer.length() > 0) + { + m_ui->receiveText->insertPlainText(lineBuffer); + // Ensure text area scrolls correctly + m_ui->receiveText->ensureCursorVisible(); + lineBuffer.clear(); + } } else if (link == currLink && holdOn) { @@ -334,46 +404,98 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) QByteArray DebugConsole::symbolNameToBytes(const QString& text) { QByteArray b; - if (text == "LF") + if (text.contains("CR+LF")) { + b.append(static_cast(0x0D)); b.append(static_cast(0x0A)); } - else if (text == "FF") + else if (text.contains("LF")) { - b.append(static_cast(0x0C)); + b.append(static_cast(0x0A)); } - else if (text == "CR") + else if (text.contains("FF")) { - b.append(static_cast(0x0D)); + b.append(static_cast(0x0C)); } - else if (text == "CR+LF") + else if (text.contains("CR")) { b.append(static_cast(0x0D)); - b.append(static_cast(0x0A)); } - else if (text == "TAB") + else if (text.contains("TAB")) { b.append(static_cast(0x09)); } - else if (text == "NUL") + else if (text.contains("NUL")) { b.append(static_cast(0x00)); } - else if (text == "ESC") + else if (text.contains("ESC")) { b.append(static_cast(0x1B)); } - else if (text == "~") + else if (text.contains("~")) { b.append(static_cast(0x7E)); } - else if (text == "") + else if (text.contains("")) { b.append(static_cast(0x20)); } return b; } +QString DebugConsole::bytesToSymbolNames(const QByteArray& b) +{ + QString text; + if (b.size() > 1 && b.contains(0x0D) && b.contains(0x0A)) + { + text = ""; + } + else if (b.contains(0x0A)) + { + text = ""; + } + else if (b.contains(0x0C)) + { + text = ""; + } + else if (b.contains(0x0D)) + { + text = ""; + } + else if (b.contains(0x09)) + { + text = ""; + } + else if (b.contains((char)0x00)) + { + text == ""; + } + else if (b.contains(0x1B)) + { + text = ""; + } + else if (b.contains(0x7E)) + { + text = "<~>"; + } + else if (b.contains(0x20)) + { + text = ""; + } + else + { + text.append(b); + } + return text; +} + +void DebugConsole::specialSymbolSelected(const QString& text) +{ + Q_UNUSED(text); + m_ui->specialCheckBox->setVisible(true); +} + void DebugConsole::appendSpecialSymbol(const QString& text) { QString line = m_ui->sendText->text(); @@ -395,8 +517,25 @@ void DebugConsole::appendSpecialSymbol(const QString& text) m_ui->sendText->setText(line); } +void DebugConsole::appendSpecialSymbol() +{ + appendSpecialSymbol(m_ui->specialComboBox->currentText()); +} + void DebugConsole::sendBytes() { + // FIXME This store settings should be removed + // once all threading issues have been resolved + // since its called in the destructor, which + // is absolutely sufficient + storeSettings(); + + // Store command history + commandHistory.append(m_ui->sendText->text()); + // Since text was just sent, we're at position commandHistory.length() + // which is the current text + commandIndex = commandHistory.length(); + if (!m_ui->sentText->isVisible()) { m_ui->sentText->setVisible(true); @@ -408,19 +547,56 @@ void DebugConsole::sendBytes() return; } + QString transmitUnconverted = m_ui->sendText->text(); + QByteArray specialSymbol; + + // Append special symbol if checkbox is checked + if (m_ui->specialCheckBox->isChecked()) + { + // Get auto-add special symbols + specialSymbol = symbolNameToBytes(m_ui->specialComboBox->currentText()); + + // Convert them if needed + if (!convertToAscii) + { + QString specialSymbolConverted; + for (int i = 0; i < specialSymbol.length(); i++) + { + QString add(" 0x%1"); + specialSymbolConverted.append(add.arg(static_cast(specialSymbol.at(i)), 2, 16, QChar('0'))); + } + specialSymbol.clear(); + specialSymbol.append(specialSymbolConverted); + } + } + QByteArray transmit; QString feedback; bool ok = true; if (convertToAscii) { // ASCII text is not converted - transmit = m_ui->sendText->text().toLatin1(); - feedback = transmit; + transmit = transmitUnconverted.toLatin1(); + // Auto-add special symbol handling + transmit.append(specialSymbol); + + QString translated; + + // Replace every occurence of a special symbol with its text name + for (int i = 0; i < transmit.size(); ++i) + { + QByteArray specialChar; + specialChar.append(transmit.at(i)); + translated.append(bytesToSymbolNames(specialChar)); + } + + feedback.append(translated); } else { // HEX symbols are converted to bytes - QString str = m_ui->sendText->text().toLatin1(); + QString str = transmitUnconverted.toLatin1(); + str.append(specialSymbol); str.remove(' '); str.remove("0x"); str.simplified(); @@ -461,16 +637,16 @@ void DebugConsole::sendBytes() if (ok && m_ui->sendText->text().toLatin1().size() > 0) { // Transmit only if conversion succeeded -// int transmitted = - currLink->writeBytes(transmit, transmit.size()); -// if (transmit.size() == transmitted) -// { - m_ui->sentText->setText(tr("Sent: ") + feedback); -// } -// else -// { -// m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size())); -// } + // int transmitted = + currLink->writeBytes(transmit, transmit.size()); + // if (transmit.size() == transmitted) + // { + m_ui->sentText->setText(tr("Sent: ") + feedback); + // } + // else + // { + // m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size())); + // } } else if (m_ui->sendText->text().toLatin1().size() > 0) { @@ -488,10 +664,18 @@ void DebugConsole::sendBytes() */ void DebugConsole::hexModeEnabled(bool mode) { - convertToAscii = !mode; - m_ui->receiveText->clear(); - m_ui->sendText->clear(); - m_ui->sentText->clear(); + if (convertToAscii == mode) + { + convertToAscii = !mode; + if (m_ui->hexCheckBox->isChecked() != mode) + { + m_ui->hexCheckBox->setChecked(mode); + } + m_ui->receiveText->clear(); + m_ui->sendText->clear(); + m_ui->sentText->clear(); + commandHistory.clear(); + } } /** @@ -499,23 +683,48 @@ void DebugConsole::hexModeEnabled(bool mode) */ void DebugConsole::MAVLINKfilterEnabled(bool filter) { - filterMAVLINK = filter; - bytesToIgnore = 0; + if (filterMAVLINK != filter) + { + filterMAVLINK = filter; + bytesToIgnore = 0; + if (m_ui->mavlinkCheckBox->isChecked() != filter) + { + m_ui->mavlinkCheckBox->setChecked(filter); + } + } } /** * @param hold Freeze the input and thus any scrolling */ void DebugConsole::hold(bool hold) { + if (holdOn != hold) + { // Check if we need to append bytes from the hold buffer if (this->holdOn && !hold) { + // TODO No conversion is done to the bytes in the hold buffer m_ui->receiveText->appendPlainText(QString(holdBuffer)); holdBuffer.clear(); lowpassDataRate = 0.0f; } this->holdOn = hold; + + // Change text interaction mode + if (hold) + { + m_ui->receiveText->setTextInteractionFlags(Qt::TextSelectableByKeyboard | Qt::TextSelectableByMouse | Qt::LinksAccessibleByKeyboard | Qt::LinksAccessibleByMouse); + } + else + { + m_ui->receiveText->setTextInteractionFlags(Qt::NoTextInteraction); + } + if (m_ui->holdCheckBox->isChecked() != hold) + { + m_ui->holdCheckBox->setChecked(hold); + } +} } /** @@ -526,12 +735,12 @@ void DebugConsole::setConnectionState(bool connected) if(connected) { m_ui->connectButton->setText(tr("Disconn.")); - m_ui->receiveText->appendHtml(QString("%2").arg(QGC::colorGreen.name(), tr("Link %1 is connected.").arg(currLink->getName()))); + m_ui->receiveText->appendHtml(QString("%2\n").arg(QGC::colorGreen.name(), tr("Link %1 is connected.").arg(currLink->getName()))); } else { m_ui->connectButton->setText(tr("Connect")); - m_ui->receiveText->appendHtml(QString("%2").arg(QGC::colorYellow.name(), tr("Link %1 is unconnected.").arg(currLink->getName()))); + m_ui->receiveText->appendHtml(QString("%2\n").arg(QGC::colorYellow.name(), tr("Link %1 is unconnected.").arg(currLink->getName()))); } } @@ -551,6 +760,75 @@ void DebugConsole::handleConnectButton() } } +void DebugConsole::keyPressEvent(QKeyEvent * event) +{ + if (event->key() == Qt::Key_Up) + { + cycleCommandHistory(true); + } + else if (event->key() == Qt::Key_Down) + { + cycleCommandHistory(false); + } + else + { + QWidget::keyPressEvent(event); + } +} + +void DebugConsole::cycleCommandHistory(bool up) +{ + // Only cycle if there is a history + if (commandHistory.length() > 0) + { + // Store current command if we're not in history yet + if (commandIndex == commandHistory.length() && up) + { + currCommand = m_ui->sendText->text(); + } + + if (up) + { + // UP + commandIndex--; + if (commandIndex >= 0) + { + m_ui->sendText->setText(commandHistory.at(commandIndex)); + } + + // If the index + } + else + { + // DOWN + commandIndex++; + if (commandIndex < commandHistory.length()) + { + m_ui->sendText->setText(commandHistory.at(commandIndex)); + } + // If the index is at history length, load the last current command + + } + + // Restore current command if we went out of history + if (commandIndex == commandHistory.length()) + { + m_ui->sendText->setText(currCommand); + } + + // If we are too far down or too far up, wrap around to current command + if (commandIndex < 0 || commandIndex > commandHistory.length()) + { + commandIndex = commandHistory.length(); + m_ui->sendText->setText(currCommand); + } + + // Bound the index + if (commandIndex < 0) commandIndex = 0; + if (commandIndex > commandHistory.length()) commandIndex = commandHistory.length(); + } +} + void DebugConsole::changeEvent(QEvent *e) { QWidget::changeEvent(e); diff --git a/src/ui/DebugConsole.h b/src/ui/DebugConsole.h index be064feb716378a0adf2227e407faeb3f9e0ef22..e0fd81dfed27734784e2b1943a6672dcdcc0f3dc 100644 --- a/src/ui/DebugConsole.h +++ b/src/ui/DebugConsole.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "LinkInterface.h" @@ -85,16 +86,29 @@ public slots: void receiveTextMessage(int id, int component, int severity, QString text); /** @brief Append a special symbol */ void appendSpecialSymbol(const QString& text); + /** @brief Append the special symbol currently selected in combo box */ + void appendSpecialSymbol(); + /** @brief A new special symbol is selected */ + void specialSymbolSelected(const QString& text); protected slots: /** @brief Draw information overlay */ void paintEvent(QPaintEvent *event); /** @brief Update traffic measurements */ void updateTrafficMeasurements(); + void loadSettings(); + void storeSettings(); protected: void changeEvent(QEvent *e); + /** @brief Convert a symbol name to the byte representation */ QByteArray symbolNameToBytes(const QString& symbol); + /** @brief Convert a symbol byte to the name */ + QString bytesToSymbolNames(const QByteArray& b); + /** @brief Handle keypress events */ + void keyPressEvent(QKeyEvent * event); + /** @brief Cycle through the command history */ + void cycleCommandHistory(bool up); QList links; LinkInterface* currLink; @@ -102,6 +116,7 @@ protected: bool holdOn; ///< Hold current view, ignore new data bool convertToAscii; ///< Convert data to ASCII bool filterMAVLINK; ///< Set true to filter out MAVLink in output + bool autoHold; ///< Auto-hold mode sets view into hold if the data rate is too high int bytesToIgnore; ///< Number of bytes to ignore char lastByte; ///< The last received byte QList sentBytes; ///< Transmitted bytes, per transmission @@ -114,7 +129,9 @@ protected: float dataRate; ///< Current data rate float lowpassDataRate; ///< Lowpass filtered data rate float dataRateThreshold; ///< Threshold where to enable auto-hold - bool autoHold; ///< Auto-hold mode sets view into hold if the data rate is too high + QStringList commandHistory; + QString currCommand; + int commandIndex; private: Ui::DebugConsole *m_ui; diff --git a/src/ui/DebugConsole.ui b/src/ui/DebugConsole.ui index 8a8fe506f1ea3e46c5ed7e86a88b50917c5a5149..e7245d5edae4dfc86dc429c7a6f5a4ad9223a319 100644 --- a/src/ui/DebugConsole.ui +++ b/src/ui/DebugConsole.ui @@ -6,7 +6,7 @@ 0 0 - 469 + 566 190 @@ -51,7 +51,7 @@ Ignore MAVLINK protocol messages in display - Hide MAVLINK + Hide MAVLink @@ -120,7 +120,7 @@ - + 5 @@ -171,11 +171,6 @@ TAB - - - NUL - - ESC @@ -193,6 +188,27 @@ + + + + + + + + :/images/actions/list-add.svg:/images/actions/list-add.svg + + + + + + + Automatically send special char at end of message + + + Auto-Add + + + diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index fc365fd162492dd77f27be60241478f02cfe8057..608dc665f356443171aea9aed7336af1ccb339c3 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -152,7 +152,7 @@ HDDisplay::~HDDisplay() QSize HDDisplay::sizeHint() const { - return QSize(400, 400.0f*(vwidth/vheight)*1.1f); + return QSize(400, 400.0f*(vwidth/vheight)*1.2f); } void HDDisplay::enableGLRendering(bool enable) @@ -293,12 +293,12 @@ void HDDisplay::addGauge() } else { - items.append(QString("%1,%2,%3,%4").arg("-180").arg(key).arg(unit).arg("+180")); + items.append(QString("%1,%2,%3,%4").arg("0").arg(key).arg(unit).arg("+100")); } } bool ok; QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), - tr("Format: min, curve name, max[,s]"), items, 0, true, &ok); + tr("Format: min, curve name, unit, max[,s]"), items, 0, true, &ok); if (ok && !item.isEmpty()) { addGauge(item); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 553c841fe45df9d650d9cb1eef09a83795c1cf9b..f46f0453c50062e26ac0962651b094cd34b98885 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -230,6 +230,10 @@ void HUD::hideEvent(QHideEvent* event) void HUD::contextMenuEvent (QContextMenuEvent* event) { QMenu menu(this); + // Update actions + enableHUDAction->setChecked(hudInstrumentsEnabled); + enableVideoAction->setChecked(videoEnabled); + menu.addAction(enableHUDAction); //menu.addAction(selectHUDColorAction); menu.addAction(enableVideoAction); @@ -266,18 +270,18 @@ void HUD::setActiveUAS(UASInterface* uas) if (this->uas != NULL) { // Disconnect any previously connected active MAV - disconnect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); - disconnect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); - disconnect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); - disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - disconnect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); + disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); + disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); + disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - disconnect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - disconnect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - disconnect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); // Try to disconnect the image link - UAS* u = dynamic_cast(uas); + UAS* u = dynamic_cast(this->uas); if (u) { disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64))); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 7e8caaf5948fc6474a49e2849ef9169134eaece3..50337188cac5d97d665b12437cb439036fcfa69c 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -22,7 +22,7 @@ #include "UDPLink.h" #include "MAVLinkProtocol.h" #include "CommConfigurationWindow.h" -#include "WaypointList.h" +#include "QGCWaypointListMulti.h" #include "MainWindow.h" #include "JoystickWidget.h" #include "GAudioOutput.h" @@ -64,7 +64,7 @@ MainWindow* MainWindow::instance() MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), toolsMenuActions(), - currentView(VIEW_ENGINEER), + currentView(VIEW_UNCONNECTED), aboutToCloseFlag(false), changingViewsFlag(false) { @@ -72,83 +72,28 @@ 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 { - if (settings.value("CURRENT_VIEW", VIEW_PILOT) != VIEW_PILOT) + // LOAD THE LAST VIEW + VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt(); + if (currentViewCandidate != VIEW_ENGINEER && + currentViewCandidate != VIEW_OPERATOR && + currentViewCandidate != VIEW_PILOT) { - currentView = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt(); + currentView = currentViewCandidate; } } - // Check if the settings exist, instantiate defaults if necessary - - // OPERATOR VIEW DEFAULT - QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - } - - // ENGINEER VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - } - - // MAVLINK VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - } - - // PILOT VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - } - - QString listKey = buildMenuKey(SUB_SECTION_CHECKED, MENU_UAS_LIST, currentView); - if (!settings.contains(listKey)) - { - settings.setValue(listKey, true); - } + setDefaultSettingsForAp(); settings.sync(); // 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->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); - - // The pilot view is not available on startup - ui.actionPilotsView->setEnabled(false); - buildCommonWidgets(); connectCommonWidgets(); @@ -217,6 +162,63 @@ MainWindow::~MainWindow() } +/** + * Set default settings for this AP type. + */ +void MainWindow::setDefaultSettingsForAp() +{ + // Check if the settings exist, instantiate defaults if necessary + + // UNCONNECTED VIEW DEFAULT + QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED); + 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 + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + + // ENABLE UAS LIST + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true); + // ENABLE HUD TOOL WIDGET + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true); + } + + // ENGINEER VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER); + 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 + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + // PILOT VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + // Enable Flight display + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true); + } +} + void MainWindow::resizeEvent(QResizeEvent * event) { Q_UNUSED(event); @@ -289,7 +291,7 @@ void MainWindow::buildCommonWidgets() controlDockWidget = new QDockWidget(tr("Control"), this); controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"); controlDockWidget->setWidget( new UASControlWidget(this) ); - addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); + addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); } if (!listDockWidget) @@ -297,15 +299,15 @@ void MainWindow::buildCommonWidgets() listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); listDockWidget->setWidget( new UASListWidget(this) ); listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET"); - addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); + addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea); } if (!waypointsDockWidget) { waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) ); waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET"); - addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); } if (!infoDockWidget) @@ -313,7 +315,7 @@ void MainWindow::buildCommonWidgets() infoDockWidget = new QDockWidget(tr("Status Details"), this); infoDockWidget->setWidget( new UASInfoWidget(this) ); infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET"); - addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); + addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea); } if (!debugConsoleDockWidget) @@ -321,7 +323,7 @@ void MainWindow::buildCommonWidgets() debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); debugConsoleDockWidget->setWidget( new DebugConsole(this) ); debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET"); - addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); + addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); } if (!logPlayerDockWidget) @@ -329,13 +331,13 @@ void MainWindow::buildCommonWidgets() logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this); logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) ); logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET"); - addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget()), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea); + addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea); } // Center widgets if (!mapWidget) { - mapWidget = new MapWidget(this); + mapWidget = new MapWidget(this); addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); } @@ -345,12 +347,13 @@ void MainWindow::buildCommonWidgets() addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); } +#ifdef MAVLINK_ENABLED_SLUGS //TODO temporaly debug if (!slugsHilSimWidget) { slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); slugsHilSimWidget->setWidget( new SlugsHilSim(this)); - addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); + addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); } //TODO temporaly debug @@ -358,8 +361,10 @@ void MainWindow::buildCommonWidgets() { slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); - addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); + addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget(bool)), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } +#endif + if (!dataplotWidget) { dataplotWidget = new QGCDataPlot2D(this); @@ -367,6 +372,7 @@ void MainWindow::buildCommonWidgets() } } + void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again @@ -437,7 +443,7 @@ void MainWindow::buildPxWidgets() detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET"); - addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea); + addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea); } if (!parametersDockWidget) @@ -445,7 +451,7 @@ void MainWindow::buildPxWidgets() parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); - addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); + addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea); } if (!watchdogControlDockWidget) @@ -453,7 +459,7 @@ void MainWindow::buildPxWidgets() watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET"); - addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea); } if (!hsiDockWidget) @@ -461,7 +467,7 @@ void MainWindow::buildPxWidgets() hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); - addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea); + addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea); } if (!headDown1DockWidget) @@ -469,7 +475,7 @@ void MainWindow::buildPxWidgets() headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET"); - addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); + addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea); } if (!headDown2DockWidget) @@ -477,7 +483,7 @@ void MainWindow::buildPxWidgets() headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); - addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); + addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); } if (!rcViewDockWidget) @@ -485,7 +491,7 @@ void MainWindow::buildPxWidgets() rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); } if (!headUpDockWidget) @@ -493,7 +499,31 @@ void MainWindow::buildPxWidgets() headUpDockWidget = new QDockWidget(tr("HUD"), this); headUpDockWidget->setWidget( new HUD(320, 240, this)); headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); - addToToolsMenu (headUpDockWidget, tr("Control Indicator"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea); + } + + if (!video1DockWidget) + { + video1DockWidget = new QDockWidget(tr("Video Stream 1"), this); + HUD* video1 = new HUD(160, 120, this); + video1->enableHUDInstruments(false); + video1->enableVideo(true); + // FIXME select video stream as well + video1DockWidget->setWidget(video1); + video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET"); + addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea); + } + + if (!video2DockWidget) + { + video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); + HUD* video2 = new HUD(160, 120, this); + video2->enableHUDInstruments(false); + video2->enableVideo(true); + // FIXME select video stream as well + video2DockWidget->setWidget(video2); + video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET"); + addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea); } // Dialogue widgets @@ -514,7 +544,7 @@ void MainWindow::buildSlugsWidgets() headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); headUpDockWidget->setWidget( new HUD(320, 240, this)); headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); - addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea); } if (!rcViewDockWidget) @@ -522,7 +552,7 @@ void MainWindow::buildSlugsWidgets() rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); } if (!slugsDataWidget) @@ -531,7 +561,7 @@ void MainWindow::buildSlugsWidgets() slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); slugsDataWidget->setWidget( new SlugsDataSensorView(this)); slugsDataWidget->setObjectName("SLUGS_DATA_DOCK_WIDGET"); - addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); + addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget(bool)), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); } if (!slugsPIDControlWidget) @@ -539,7 +569,7 @@ void MainWindow::buildSlugsWidgets() slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); slugsPIDControlWidget->setObjectName("SLUGS_PID_CONTROL_DOCK_WIDGET"); - addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); + addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); } if (!slugsHilSimWidget) @@ -547,7 +577,7 @@ void MainWindow::buildSlugsWidgets() slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); slugsHilSimWidget->setWidget( new SlugsHilSim(this)); slugsHilSimWidget->setObjectName("SLUGS_HIL_SIM_DOCK_WIDGET"); - addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); + addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); } if (!slugsCamControlWidget) @@ -555,7 +585,7 @@ void MainWindow::buildSlugsWidgets() slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET"); - addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); + addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget(bool)), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } } @@ -568,15 +598,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, QAction* tempAction; - // Add the separator that will separate tools from central Widgets - if (!toolsMenuActions[CENTRAL_SEPARATOR]) - { - tempAction = ui.menuTools->addSeparator(); - toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; - tempAction->setData(CENTRAL_SEPARATOR); - } +// Not needed any more - separate menu now available + +// // Add the separator that will separate tools from central Widgets +// if (!toolsMenuActions[CENTRAL_SEPARATOR]) +// { +// tempAction = ui.menuTools->addSeparator(); +// toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; +// tempAction->setData(CENTRAL_SEPARATOR); +// } - tempAction = ui.menuTools->addAction(title); + tempAction = ui.menuMain->addAction(title); tempAction->setCheckable(true); tempAction->setData(centralWidget); @@ -598,13 +630,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, } // connect the action - connect(tempAction,SIGNAL(triggered()),this, slotName); + connect(tempAction,SIGNAL(triggered(bool)),this, slotName); } void MainWindow::showCentralWidget() { QAction* senderAction = qobject_cast(sender()); + + // Block sender action while manipulating state + senderAction->blockSignals(true); + int tool = senderAction->data().toInt(); QString chKey; @@ -612,7 +648,6 @@ void MainWindow::showCentralWidget() if (senderAction && dockWidgets[tool]) { - // uncheck all central widget actions QHashIterator i(toolsMenuActions); while (i.hasNext()) @@ -621,7 +656,11 @@ void MainWindow::showCentralWidget() //qDebug() << "shCW" << i.key() << "read"; if (i.value() && i.value()->data().toInt() > 255) { + // Block signals and uncheck action + // firing would be unneccesary + i.value()->blockSignals(true); i.value()->setChecked(false); + i.value()->blockSignals(false); // update the settings chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); @@ -640,6 +679,9 @@ void MainWindow::showCentralWidget() chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); settings.setValue(chKey,true); + // Unblock sender action + senderAction->blockSignals(false); + presentView(); } } @@ -699,39 +741,97 @@ void MainWindow::addToToolsMenu ( QWidget* widget, } else { - tempAction->setChecked(settings.value(chKey).toBool()); + tempAction->setChecked(settings.value(chKey, false).toBool()); widget->setVisible(settings.value(chKey, false).toBool()); } // connect the action - connect(tempAction,SIGNAL(triggered()),this, slotName); + connect(tempAction,SIGNAL(toggled(bool)),this, slotName); + + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool))); -// connect(qobject_cast (dockWidgets[tool]), -// SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); + // connect(qobject_cast (dockWidgets[tool]), + // SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); connect(qobject_cast (dockWidgets[tool]), SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); - } -void MainWindow::showToolWidget() +void MainWindow::showToolWidget(bool visible) { - QAction* temp = qobject_cast(sender()); - int tool = temp->data().toInt(); - - if (temp && dockWidgets[tool]) + if (!aboutToCloseFlag && !changingViewsFlag) { - if (temp->isChecked()) + QAction* action = qobject_cast(sender()); + + // Prevent this to fire if undocked + if (action) { - addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); - qobject_cast(dockWidgets[tool])->show(); + int tool = action->data().toInt(); + + QDockWidget* dockWidget = qobject_cast (dockWidgets[tool]); + + qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible(); + if (dockWidget && dockWidget->isVisible() != visible) + { + if (visible) + { + qDebug() << "DOCK WIDGET ADDED"; + addDockWidget(dockWidgetLocations[tool], dockWidget); + dockWidget->show(); + } + else + { + qDebug() << "DOCK WIDGET REMOVED"; + removeDockWidget(dockWidget); + //dockWidget->hide(); + } + + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == dockWidget) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,visible); + qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + break; + } + } + } } - else + + QDockWidget* dockWidget = qobject_cast(QObject::sender()); + + qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible; + + if (dockWidget) { - removeDockWidget(qobject_cast(dockWidgets[tool])); + // Get action + int tool = dockWidgets.key(dockWidget); + + qDebug() << "Updating widget setting" << tool << "to" << visible; + + QAction* action = toolsMenuActions[tool]; + action->blockSignals(true); + action->setChecked(visible); + action->blockSignals(false); + + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == dockWidget) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,visible); + qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + break; + } + } } } - } @@ -743,6 +843,8 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool(); + qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible; + if (tempWidget) { toolsMenuActions[widget]->setChecked(tempVisible); @@ -761,12 +863,14 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) // } // } - if ((tempWidget != NULL) && tempVisible) + if (tempWidget != NULL) { - addDockWidget(tempLocation, tempWidget); - tempWidget->show(); + if (tempVisible) + { + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } } - } QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view) @@ -774,15 +878,17 @@ 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; - return (QString::number(apType) + "/" + - QString::number(SECTION_MENU) + "/" + - QString::number(view) + "/" + - QString::number(tool) + "/" + - QString::number(section) + "/" ); + apType = 1; + + return (QString::number(apType) + "_" + + QString::number(SECTION_MENU) + "_" + + QString::number(view) + "_" + + QString::number(tool) + "_" + + QString::number(section) + "_" ); } void MainWindow::closeEvent(QCloseEvent *event) @@ -800,6 +906,29 @@ void MainWindow::closeEvent(QCloseEvent *event) QMainWindow::closeEvent(event); } +void MainWindow::showDockWidget (bool vis) +{ + if (!aboutToCloseFlag && !changingViewsFlag) + { + QDockWidget* temp = qobject_cast(sender()); + + if (temp) + { + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } + } + } +} void MainWindow::updateVisibilitySettings (bool vis) { @@ -810,9 +939,11 @@ void MainWindow::updateVisibilitySettings (bool vis) if (temp) { QHashIterator i(dockWidgets); - while (i.hasNext()) { + while (i.hasNext()) + { i.next(); - if ((static_cast (dockWidgets[i.key()])) == temp) { + if ((static_cast (dockWidgets[i.key()])) == temp) + { QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); settings.setValue(chKey,vis); toolsMenuActions[i.key()]->setChecked(vis); @@ -1120,6 +1251,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())); @@ -1132,13 +1293,14 @@ 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 connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView())); connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); + connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView())); connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); @@ -1260,6 +1422,9 @@ void MainWindow::addLink(LinkInterface *link) CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); ui.menuNetwork->addAction(commWidget->getAction()); + // Error handling + connect(link, SIGNAL(communicationError(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); + //qDebug() << "ADDING LINK:" << link->getName() << "ACTION IS: " << commWidget->getAction(); // Special case for simulationlink @@ -1278,6 +1443,18 @@ void MainWindow::setActiveUAS(UASInterface* uas) if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true); } +void MainWindow::UASSpecsChanged(int uas) +{ + UASInterface* activeUAS = UASManager::instance()->getActiveUAS(); + if (activeUAS) + { + if (activeUAS->getUASID() == uas) + { + ui.menuUnmanned_System->setTitle(activeUAS->getUASName()); + } + } +} + void MainWindow::UASCreated(UASInterface* uas) { @@ -1285,8 +1462,19 @@ void MainWindow::UASCreated(UASInterface* uas) if (uas != NULL) { - // The pilot view was not available on startup, enable it now + // Set default settings + setDefaultSettingsForAp(); + + // The pilot, operator and engineer views were not available on startup, enable them now 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 @@ -1318,6 +1506,7 @@ void MainWindow::UASCreated(UASInterface* uas) QAction* uasAction = new QAction(icon, tr("Select %1 for control").arg(uas->getUASName()), ui.menuConnected_Systems); connect(uas, SIGNAL(systemRemoved()), uasAction, SLOT(deleteLater())); connect(uasAction, SIGNAL(triggered()), uas, SLOT(setSelected())); + connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int))); ui.menuConnected_Systems->addAction(uasAction); @@ -1380,6 +1569,7 @@ void MainWindow::UASCreated(UASInterface* uas) // } // } // } + } break; default: @@ -1400,8 +1590,6 @@ void MainWindow::UASCreated(UASInterface* uas) connectPxActions(); } break; - - loadOperatorView(); } // Change the view only if this is the first UAS @@ -1415,19 +1603,31 @@ void MainWindow::UASCreated(UASInterface* uas) // Load last view if setting is present if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) { + clearView(); int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt(); - currentView = (VIEW_SECTIONS) view; - presentView(); - - // Restore the widget positions and size - if (settings.contains(getWindowStateKey())) + switch (view) { - restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); + case VIEW_ENGINEER: + loadEngineerView(); + break; + case VIEW_MAVLINK: + loadMAVLinkView(); + break; + case VIEW_PILOT: + loadPilotView(); + break; + case VIEW_UNCONNECTED: + loadUnconnectedView(); + break; + case VIEW_OPERATOR: + default: + loadOperatorView(); + break; } } else { - loadEngineerView(); + loadOperatorView(); } } @@ -1445,7 +1645,7 @@ void MainWindow::UASCreated(UASInterface* uas) void MainWindow::clearView() { // Save current state - if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); settings.setValue(getWindowGeometryKey(), saveGeometry()); QAction* temp; @@ -1458,12 +1658,12 @@ void MainWindow::clearView() if (temp) { - //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); + qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); settings.setValue(chKey,temp->isChecked()); } else { - //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; + qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; settings.setValue(chKey,false); } } @@ -1480,11 +1680,9 @@ void MainWindow::clearView() if (dockWidget) { // Remove dock widget from main window - //this->removeDockWidget(dockWidget); - dockWidget->setVisible(false); - // Deletion of dockWidget would also delete all child - // widgets of dockWidget - // Is there a way to unset a widget from QDockWidget? + removeDockWidget(dockWidget); + dockWidget->hide(); + //dockWidget->setVisible(false); } } changingViewsFlag = false; @@ -1494,11 +1692,10 @@ void MainWindow::loadEngineerView() { if (currentView != VIEW_ENGINEER) { - clearView(); - - currentView = VIEW_ENGINEER; - - presentView(); + clearView(); + currentView = VIEW_ENGINEER; + ui.actionEngineersView->setChecked(true); + presentView(); } } @@ -1506,11 +1703,21 @@ void MainWindow::loadOperatorView() { if (currentView != VIEW_OPERATOR) { - clearView(); - - currentView = VIEW_OPERATOR; + clearView(); + currentView = VIEW_OPERATOR; + ui.actionOperatorsView->setChecked(true); + presentView(); + } +} - presentView(); +void MainWindow::loadUnconnectedView() +{ + if (currentView != VIEW_UNCONNECTED) + { + clearView(); + currentView = VIEW_UNCONNECTED; + ui.actionUnconnectedView->setChecked(true); + presentView(); } } @@ -1518,11 +1725,10 @@ void MainWindow::loadPilotView() { if (currentView != VIEW_PILOT) { - clearView(); - - currentView = VIEW_PILOT; - - presentView(); + clearView(); + currentView = VIEW_PILOT; + ui.actionPilotsView->setChecked(true); + presentView(); } } @@ -1530,11 +1736,10 @@ void MainWindow::loadMAVLinkView() { if (currentView != VIEW_MAVLINK) { - clearView(); - - currentView = VIEW_MAVLINK; - - presentView(); + clearView(); + currentView = VIEW_MAVLINK; + ui.actionMavlinkView->setChecked(true); + presentView(); } } @@ -1565,7 +1770,6 @@ void MainWindow::presentView() showTheCentralWidget(CENTRAL_DATA_PLOT, currentView); - // Show docked widgets based on current view and autopilot type // UAS CONTROL @@ -1640,8 +1844,29 @@ void MainWindow::presentView() // MAVLINK LOG PLAYER showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView); + // VIDEO 1 + showTheWidget(MENU_VIDEO_STREAM_1, currentView); + + // VIDEO 2 + showTheWidget(MENU_VIDEO_STREAM_2, currentView); + this->show(); + // Restore window state + if (UASManager::instance()->getUASList().count() > 0) + { + // Restore the mainwindow size + if (settings.contains(getWindowGeometryKey())) + { + restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); + } + + // Restore the widget positions and size + if (settings.contains(getWindowStateKey())) + { + restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); + } + } } void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) @@ -1649,15 +1874,17 @@ void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SE bool tempVisible; QWidget* tempWidget = dockWidgets[centralWidget]; - tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); - //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; + tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); + qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; if (toolsMenuActions[centralWidget]) { + qDebug() << "SETTING TO:" << tempVisible; toolsMenuActions[centralWidget]->setChecked(tempVisible); } if (centerStack && tempWidget && tempVisible) { + qDebug() << "ACTIVATING MAIN WIDGET"; centerStack->setCurrentWidget(tempWidget); } } diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index df1cfb0a5a7fe579b857b3e67369aae9735aae3a..4946deba02cc15b31b32978c2a43e14e445eee00 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -108,10 +108,14 @@ public slots: void setActiveUAS(UASInterface* uas); /** @brief Add a new UAS */ void UASCreated(UASInterface* uas); + /** @brief Update system specs of a UAS */ + void UASSpecsChanged(int uas); void startVideoCapture(); void stopVideoCapture(); void saveScreen(); + /** @brief Load default view when no MAV is connected */ + void loadUnconnectedView(); /** @brief Load view for pilot */ void loadPilotView(); /** @brief Load view for engineer */ @@ -166,7 +170,7 @@ public slots: * It shows the QDockedWidget based on the action sender * */ - void showToolWidget(); + void showToolWidget(bool visible); /** * @brief Shows a Widget from the center stack based on the action sender @@ -177,8 +181,10 @@ public slots: */ void showCentralWidget(); + /** @brief Change actively a QDockWidgets visibility by an action */ + void showDockWidget(bool vis); /** @brief Updates a QDockWidget's checked status based on its visibility */ - void updateVisibilitySettings (bool vis); + void updateVisibilitySettings(bool vis); /** @brief Updates a QDockWidget's location */ void updateLocationSettings (Qt::DockWidgetArea location); @@ -187,6 +193,9 @@ protected: MainWindow(QWidget *parent = 0); + /** @brief Set default window settings for the current autopilot type */ + void setDefaultSettingsForAp(); + // These defines are used to save the settings when selecting with // which widgets populate the views // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over @@ -212,6 +221,8 @@ protected: MENU_SLUGS_HIL, MENU_SLUGS_CAMERA, MENU_MAVLINK_LOG_PLAYER, + MENU_VIDEO_STREAM_1, + MENU_VIDEO_STREAM_2, CENTRAL_SEPARATOR= 255, // do not change CENTRAL_LINECHART, CENTRAL_PROTOCOL, @@ -238,6 +249,7 @@ protected: VIEW_OPERATOR, VIEW_PILOT, VIEW_MAVLINK, + VIEW_UNCONNECTED, ///< View in unconnected mode, when no UAS is available } VIEW_SECTIONS; @@ -368,6 +380,8 @@ protected: QPointer watchdogControlDockWidget; QPointer headUpDockWidget; + QPointer video1DockWidget; + QPointer video2DockWidget; QPointer logPlayerDockWidget; QPointer hsiDockWidget; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 5808be0b0053bb85beee4a4b984c4d7dceda6936..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 + @@ -100,7 +101,7 @@ - Tools + Widgets @@ -121,11 +122,18 @@ + + + + + Main + + @@ -296,6 +304,9 @@ Operator + + Meta+O + @@ -308,6 +319,9 @@ Engineer + + Meta+E + @@ -320,6 +334,9 @@ Mavlink + + Meta+M + @@ -341,6 +358,9 @@ Pilot + + Meta+P + @@ -379,6 +399,36 @@ QGroundControl global settings + + + true + + + + :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg + + + Unconnected + + + 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..de875253764f44b9d1b9ad9c05d8b312a23655a3 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,7 +636,8 @@ 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->getWaypointManager(), SIGNAL(waypointListChanged()), this, SLOT(redoWaypoints())); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int))); } void MapWidget::updateWaypointList(int uas) @@ -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,78 @@ 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*))); + + updateSystemSpecs(mav->getUASID()); + updateSelectedSystem(mav->getUASID()); + mc->updateRequest(waypointPath->boundingBox().toRect()); + } +} + +void MapWidget::updateSystemSpecs(int uas) +{ + foreach (qmapcontrol::Point* p, uasIcons.values()) + { + MAV2DIcon* icon = dynamic_cast(p); + if (icon && icon->getUASId() == uas) + { + // Set new airframe + icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe()); + icon->drawIcon(); + } + } +} + +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 +814,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()) - { - updatePosition(0, lat, lon); - } - else + // Limit the position update rate + quint64 currTime = MG::TIME::getGroundTimeNow(); + if (currTime - lastUpdate > 120) { - // 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 +958,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 +991,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 +1001,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 +1038,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..1490e593fbbf6c5eabeafc203d3a90a472594805 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -68,6 +68,12 @@ public: public slots: void addUAS(UASInterface* uas); void activeUASSet(UASInterface* uas); + /** @brief Update the system specs of one system */ + void updateSystemSpecs(int 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 +91,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/QGCWaypointListMulti.cc b/src/ui/QGCWaypointListMulti.cc new file mode 100644 index 0000000000000000000000000000000000000000..f8f07f06475fba3bb8320e61143a62fddaf88c55 --- /dev/null +++ b/src/ui/QGCWaypointListMulti.cc @@ -0,0 +1,62 @@ +#include "QGCWaypointListMulti.h" +#include "ui_QGCWaypointListMulti.h" +#include "UASManager.h" + +QGCWaypointListMulti::QGCWaypointListMulti(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCWaypointListMulti) +{ + ui->setupUi(this); + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(systemCreated(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), this, SLOT(systemSetActive(int))); +} + +void QGCWaypointListMulti::systemDeleted(QObject* uas) +{ + UASInterface* mav = dynamic_cast(uas); + if (mav) + { + int id = mav->getUASID(); + WaypointList* list = lists.value(id, NULL); + if (list) + { + delete list; + lists.remove(id); + } + } +} + +void QGCWaypointListMulti::systemCreated(UASInterface* uas) +{ + WaypointList* list = new WaypointList(ui->stackedWidget, uas); + lists.insert(uas->getUASID(), list); + ui->stackedWidget->addWidget(list); + // Ensure widget is deleted when system is deleted + connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(systemDeleted(QObject*))); +} + +void QGCWaypointListMulti::systemSetActive(int uas) +{ + WaypointList* list = lists.value(uas, NULL); + if (list) + { + ui->stackedWidget->setCurrentWidget(list); + } +} + +QGCWaypointListMulti::~QGCWaypointListMulti() +{ + delete ui; +} + +void QGCWaypointListMulti::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} diff --git a/src/ui/QGCWaypointListMulti.h b/src/ui/QGCWaypointListMulti.h new file mode 100644 index 0000000000000000000000000000000000000000..ec9847f5df8fc0359db25e2cb8bcea06b8e86949 --- /dev/null +++ b/src/ui/QGCWaypointListMulti.h @@ -0,0 +1,35 @@ +#ifndef QGCWAYPOINTLISTMULTI_H +#define QGCWAYPOINTLISTMULTI_H + +#include +#include + +#include "WaypointList.h" +#include "UASInterface.h" + +namespace Ui { + class QGCWaypointListMulti; +} + +class QGCWaypointListMulti : public QWidget +{ + Q_OBJECT + +public: + explicit QGCWaypointListMulti(QWidget *parent = 0); + ~QGCWaypointListMulti(); + +public slots: + void systemDeleted(QObject* uas); + void systemCreated(UASInterface* uas); + void systemSetActive(int uas); + +protected: + void changeEvent(QEvent *e); + QMap lists; + +private: + Ui::QGCWaypointListMulti *ui; +}; + +#endif // QGCWAYPOINTLISTMULTI_H diff --git a/src/ui/QGCWaypointListMulti.ui b/src/ui/QGCWaypointListMulti.ui new file mode 100644 index 0000000000000000000000000000000000000000..166688410d240f93360c1f1cfa8b024968b8aa1e --- /dev/null +++ b/src/ui/QGCWaypointListMulti.ui @@ -0,0 +1,27 @@ + + + QGCWaypointListMulti + + + + 0 + 0 + 400 + 300 + + + + Form + + + + 0 + + + + + + + + + diff --git a/src/ui/UASControl.ui b/src/ui/UASControl.ui index e506dbc1622d82c1b45b68137983b7f91e2157f1..a24e5e17a7963a76b829efee4cc2a7887bde7f2c 100644 --- a/src/ui/UASControl.ui +++ b/src/ui/UASControl.ui @@ -62,6 +62,12 @@ 10 + + Currently controlled system + + + Currently controlled system + UNCONNECTED @@ -99,6 +105,12 @@ 16 + + Liftoff / Launch + + + Liftoff / Launch + Start @@ -116,6 +128,12 @@ 16 + + Fly straight to landing spot + + + Fly straight to landing spot + Land @@ -133,6 +151,12 @@ 16 + + Only active on the ground: Poweroff system + + + Only active on the ground: Poweroff system + Halt @@ -152,6 +176,12 @@ 16 + + Select MAV operation mode + + + Select MAV operation mode + @@ -162,6 +192,12 @@ 16 + + Transmit and enable mode on MAV + + + Transmit and enable mode on MAV + Set @@ -179,6 +215,12 @@ 10 + + Status label + + + Status label + No actions executed so far @@ -208,6 +250,12 @@ 30 + + Main control button + + + Main control button + Activate Engine diff --git a/src/ui/UASView.ui b/src/ui/UASView.ui index eecc77b37cd63d72c312240d3f096d2152a27a07..079999ec90bed3d922c985c5ffe101a3d29b3dd2 100644 --- a/src/ui/UASView.ui +++ b/src/ui/UASView.ui @@ -6,12 +6,12 @@ 0 0 - 310 + 260 111 - + 0 0 @@ -22,6 +22,12 @@ 0 + + + 360 + 16777215 + + Form @@ -29,10 +35,14 @@ QWidget#colorIcon {} QWidget { -background-color: none; +background-color: #050508; color: #DDDDDF; -border-color: #EEEEEE; -background-clip: margin; +background-clip: border; +font-size: 11px; +} + +QLabel { +background-color: transparent; } QLabel#nameLabel { @@ -101,7 +111,7 @@ QToolButton#typeButton { border: 0px solid #999999; border-radius: 5px; min-width:44px; - max-width: 44px; + max-width: 80px; min-height: 44px; max-height: 44px; padding: 0px; @@ -114,7 +124,7 @@ QPushButton { border: 1px solid #999999; border-radius: 10px; min-width: 20px; - max-width: 40px; + max-width: 80px; min-height: 16px; max-height: 16px; padding: 2px; @@ -146,10 +156,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 +179,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 +255,7 @@ QMenu::separator { - + 4 @@ -251,7 +275,7 @@ QMenu::separator { - 44 + 80 44 @@ -261,6 +285,12 @@ QMenu::separator { 30 + + System Type + + + System Type + ... @@ -281,9 +311,12 @@ QMenu::separator { Qt::Horizontal + + QSizePolicy::MinimumExpanding + - 6 + 4 88 @@ -305,6 +338,12 @@ QMenu::separator { true + + System Name + + + System Name + UAS001 @@ -327,7 +366,7 @@ QMenu::separator { - Waiting for first update.. + @@ -341,6 +380,12 @@ QMenu::separator { false + + Remaining flight time + + + Remaining flight time + 00:00:00 @@ -356,6 +401,12 @@ QMenu::separator { false + + Uptime + + + Uptime + 00:00:00 @@ -365,9 +416,15 @@ QMenu::separator { - 8 + -1 + + Current throttle + + + Current throttle + 0 @@ -383,6 +440,12 @@ QMenu::separator { false + + Altitude + + + Altitude + 00.00 m @@ -398,6 +461,12 @@ QMenu::separator { false + + Ground Speed + + + Ground Speed + 00.0 m/s @@ -423,6 +492,12 @@ QMenu::separator { 40 + + Heartbeat + + + Heartbeat + @@ -444,9 +519,15 @@ QMenu::separator { - 6 + -1 + + Battery Fuel + + + Battery Fuel + 0 @@ -466,7 +547,7 @@ QMenu::separator { - UNINIT + @@ -480,15 +561,21 @@ QMenu::separator { false + + Current Waypoint + + + Current Waypoint + - WPX + --- Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - + @@ -510,35 +597,14 @@ QMenu::separator { false - - 00.0 00.0 00.0 m - - - - - - - - 0 - 12 - + + Current Position - - - 16777215 - 12 - - - - - -1 - 50 - false - false - + + Current Position - 00 00 00 N 00 00 00 E + 00.0 00.0 00.0 m @@ -552,11 +618,11 @@ QMenu::separator { - 10 + -1 - Unknown status + @@ -576,6 +642,12 @@ QMenu::separator { 22 + + Liftoff / Launch + + + Liftoff / Launch + @@ -593,6 +665,12 @@ QMenu::separator { 22 + + Loiter / Wait at current position + + + Loiter / Wait at current position + @@ -610,6 +688,12 @@ QMenu::separator { 22 + + Continue flightplan + + + Continue flightplan + @@ -627,6 +711,12 @@ QMenu::separator { 22 + + Fly straight to landing location + + + Fly straight to landing location + @@ -638,6 +728,12 @@ QMenu::separator { + + Only in standby mode: Power off system + + + Only in standby mode: Power off system + @@ -655,6 +751,12 @@ QMenu::separator { 22 + + Emergency land system at closest possible site + + + Emergency land system at closest possible site + @@ -672,6 +774,12 @@ QMenu::separator { 22 + + Kill immediately all onboard power + + + Kill immediately all onboard power + 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..9ea5f05b8c128daa0213df32e9bcf6e38c2f3e40 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -57,8 +57,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : listLayout->setAlignment(Qt::AlignTop); m_ui->listWidget->setLayout(listLayout); - this->uas = NULL; - // ADD WAYPOINT // Connect add action, set right button icon and connect action to this class connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered())); @@ -77,7 +75,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints())); connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints())); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); @@ -129,6 +127,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 +213,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 +292,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 +307,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 +357,6 @@ void WaypointList::waypointListChanged() loadFileGlobalWP = false; - //} } void WaypointList::moveUp(Waypoint* wp) @@ -499,29 +447,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..411429ee45b5d351a683dab9139289e9d9168ff3 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -170,9 +170,15 @@ QProgressBar::chunk#thrustBar { + + Qt::TabFocus + Currently selected waypoint + + Currently selected waypoint + @@ -211,6 +217,12 @@ QProgressBar::chunk#thrustBar { 0 + + Action at Waypoint + + + Action at Waypoint + @@ -221,6 +233,12 @@ QProgressBar::chunk#thrustBar { 0 + + Coordinate frame + + + Coordinate frame + @@ -231,9 +249,24 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + Position X coordinate + + Position X corrdinate + + + false + + + true + + + true + N @@ -259,8 +292,14 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + - Position Y coordinate + Position Y/Longitude coordinate + + + Position Y/Longitude coordinate E @@ -290,8 +329,14 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + - Position Z coordinate (negative) + Position Z coordinate (local frame, negative) + + + D @@ -318,6 +363,15 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + + + Latitude in degrees + + + Latitude in degrees + lat @@ -333,6 +387,9 @@ QProgressBar::chunk#thrustBar { 90.000000000000000 + + 0.000010000000000 + @@ -343,6 +400,15 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::WheelFocus + + + Longitude in degrees + + + Longitude in degrees + lon @@ -358,6 +424,9 @@ QProgressBar::chunk#thrustBar { 180.000000000000000 + + 0.000010000000000 + @@ -368,6 +437,12 @@ QProgressBar::chunk#thrustBar { 0 + + Altitude in meters + + + Altitude in meters + alt @@ -393,8 +468,14 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + - Yaw angle + Rotary wing only: Desired yaw angle at waypoint + + + Rotary wing only: Desired yaw angle at waypoint true @@ -421,8 +502,14 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + - Orbit radius + Orbit (navigate waypoint) /Loiter (loiter waypoint) radius + + + Orbit (navigate waypoint) /Loiter (loiter waypoint) radius m @@ -449,11 +536,14 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Time in milliseconds that the MAV has to stay inside the orbit before advancing - + Time in milliseconds that the MAV has to stay inside the orbit before advancing ms @@ -477,9 +567,15 @@ QProgressBar::chunk#thrustBar { 0 + + Qt::StrongFocus + Take off angle + + Take off angle + @@ -493,6 +589,9 @@ QProgressBar::chunk#thrustBar { Automatically continue after this waypoint + + Automatically continue after this waypoint + @@ -506,8 +605,14 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + - Move Up + Move Up in List + + + Move Up in List @@ -526,8 +631,14 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + - Move Down + Move Down in List + + + Move Down in List @@ -546,6 +657,9 @@ QProgressBar::chunk#thrustBar { 22 + + Qt::NoFocus + Delete diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index 9dc23f98bac27734eb52facb841a61ea057123db..dad5c8a3fbbeb3953b66b90f62bc96e00f5a2362 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -221,6 +221,7 @@ void QGCToolWidget::addParam() if (ui->hintLabel) { ui->hintLabel->deleteLater(); + ui->hintLabel = NULL; } toolLayout->addWidget(slider); slider->startEditMode(); @@ -232,6 +233,7 @@ void QGCToolWidget::addAction() if (ui->hintLabel) { ui->hintLabel->deleteLater(); + ui->hintLabel = NULL; } toolLayout->addWidget(button); button->startEditMode(); @@ -242,6 +244,7 @@ void QGCToolWidget::addToolWidget(QGCToolWidgetItem* widget) if (ui->hintLabel) { ui->hintLabel->deleteLater(); + ui->hintLabel = NULL; } toolLayout->addWidget(widget); } diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index 8920f76d996e626505b450d55620cd144e1f3630..18440d69719dbaec57658f31f865d430760d3365 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -1,23 +1,34 @@ #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), + airframe(uas->getAirframe()), + 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), + airframe(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 +38,237 @@ 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); - // 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) + // Rotate by yaw + painter.translate(radius/2, radius/2); + + // 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 (airframe) { - QPen pen2(Qt::red); - pen2.setWidth(2); - painter.setPen(pen2); - } - painter.setBrush(Qt::NoBrush); + case UASInterface::QGC_AIRFRAME_PREDATOR: + case UASInterface::QGC_AIRFRAME_REAPER: + { + // DRAW AIRPLANE + + // 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 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); + 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 UASInterface::QGC_AIRFRAME_MIKROKOPTER: + case UASInterface::QGC_AIRFRAME_CHEETAH: + { + // 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 UASInterface::QGC_AIRFRAME_EASYSTAR: + case UASInterface::QGC_AIRFRAME_MERLIN: + case UASInterface::QGC_AIRFRAME_TWINSTAR: + case UASInterface::QGC_AIRFRAME_PTERYX: + { + // DRAW AIRPLANE + + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + float iconSize = radius*0.7f; + + QPolygonF poly(32); + poly.replace(0, QPointF(0.000000f*iconSize, 0.362319f*iconSize)); + poly.replace(1, QPointF(0.018116f*iconSize, 0.340580f*iconSize)); + poly.replace(2, QPointF(0.028986f*iconSize, 0.318841f*iconSize)); + poly.replace(3, QPointF(0.036232f*iconSize, 0.166667f*iconSize)); + poly.replace(4, QPointF(0.326087f*iconSize, 0.170290f*iconSize)); + poly.replace(5, QPointF(0.420290f*iconSize, 0.130435f*iconSize)); + poly.replace(6, QPointF(0.456522f*iconSize, 0.108696f*iconSize)); + poly.replace(7, QPointF(0.500000f*iconSize, 0.000000f*iconSize)); + poly.replace(8, QPointF(0.456522f*iconSize, 0.021739f*iconSize)); + poly.replace(9, QPointF(0.391304f*iconSize, 0.021739f*iconSize)); + poly.replace(10, QPointF(0.028986f*iconSize, 0.021739f*iconSize)); + poly.replace(11, QPointF(0.021739f*iconSize, -0.239130f*iconSize)); + poly.replace(12, QPointF(0.094203f*iconSize, -0.246377f*iconSize)); + poly.replace(13, QPointF(0.123188f*iconSize, -0.268116f*iconSize)); + poly.replace(14, QPointF(0.144928f*iconSize, -0.304348f*iconSize)); + poly.replace(15, QPointF(0.000000f*iconSize, -0.307971f*iconSize)); + poly.replace(16, QPointF(0.000000f*iconSize, -0.307971f*iconSize)); + poly.replace(17, QPointF(-0.144928f*iconSize, -0.304348f*iconSize)); + poly.replace(18, QPointF(-0.123188f*iconSize, -0.268116f*iconSize)); + poly.replace(19, QPointF(-0.094203f*iconSize, -0.246377f*iconSize)); + poly.replace(20, QPointF(-0.021739f*iconSize, -0.239130f*iconSize)); + poly.replace(21, QPointF(-0.028986f*iconSize, 0.021739f*iconSize)); + poly.replace(22, QPointF(-0.391304f*iconSize, 0.021739f*iconSize)); + poly.replace(23, QPointF(-0.456522f*iconSize, 0.021739f*iconSize)); + poly.replace(24, QPointF(-0.500000f*iconSize, 0.000000f*iconSize)); + poly.replace(25, QPointF(-0.456522f*iconSize, 0.108696f*iconSize)); + poly.replace(26, QPointF(-0.420290f*iconSize, 0.130435f*iconSize)); + poly.replace(27, QPointF(-0.326087f*iconSize, 0.170290f*iconSize)); + poly.replace(28, QPointF(-0.036232f*iconSize, 0.166667f*iconSize)); + poly.replace(29, QPointF(-0.028986f*iconSize, 0.318841f*iconSize)); + poly.replace(30, QPointF(-0.018116f*iconSize, 0.340580f*iconSize)); + poly.replace(31, QPointF(-0.000000f*iconSize, 0.362319f*iconSize)); + + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + } + break; + case UASInterface::QGC_AIRFRAME_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..071d71bf5adba745bd37a45ca4ee4376d4981111 100644 --- a/src/ui/map/MAV2DIcon.h +++ b/src/ui/map/MAV2DIcon.h @@ -1,32 +1,45 @@ #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_COAXIAL, + MAV_ICON_HELICOPTER, + } 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 +50,26 @@ public: */ virtual void setPen(QPen* pen); + /** @brief Mark this system as selected */ + void setSelectedUAS(bool selected); void setYaw(float yaw); + /** @brief Set the airframe this MAV uses */ + void setAirframe(int airframe) { this->airframe = airframe; } + + /** @brief Get system id */ + int getUASId() const { return uasid; } + void drawIcon(QPen* pen); + void drawIcon() { drawIcon(mypen); } 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 + int airframe; ///< The specific type of airframe used + 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/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 27dc8acf94cfbfe6a0939e6b6f3d5f1b24e9d0b5..0bfb64ca4beb80e5c669155058c012a79d744543 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -308,7 +308,7 @@ Pixhawk3DWidget::insertWaypoint(void) if (wp) { wp->setFrame(frame); - uas->getWaypointManager().addWaypoint(wp); + uas->getWaypointManager()->addWaypoint(wp); } } } @@ -325,7 +325,7 @@ Pixhawk3DWidget::setWaypoint(void) if (uas) { const QVector waypoints = - uas->getWaypointManager().getWaypointList(); + uas->getWaypointManager()->getWaypointList(); Waypoint* waypoint = waypoints.at(selectedWpIndex); if (frame == MAV_FRAME_GLOBAL) @@ -366,7 +366,7 @@ Pixhawk3DWidget::deleteWaypoint(void) { if (uas) { - uas->getWaypointManager().removeWaypoint(selectedWpIndex); + uas->getWaypointManager()->removeWaypoint(selectedWpIndex); } } @@ -377,7 +377,7 @@ Pixhawk3DWidget::setWaypointAltitude(void) { bool ok; const QVector waypoints = - uas->getWaypointManager().getWaypointList(); + uas->getWaypointManager()->getWaypointList(); Waypoint* waypoint = waypoints.at(selectedWpIndex); double altitude = waypoint->getZ(); @@ -409,10 +409,10 @@ Pixhawk3DWidget::clearAllWaypoints(void) if (uas) { const QVector waypoints = - uas->getWaypointManager().getWaypointList(); + uas->getWaypointManager()->getWaypointList(); for (int i = waypoints.size() - 1; i >= 0; --i) { - uas->getWaypointManager().removeWaypoint(i); + uas->getWaypointManager()->removeWaypoint(i); } } } diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 1ec7fe25453215a5db76d2f4ffce618fbe737734..c72e3ee36cf70b0f1ff365eb9fa20121d97e832f 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -151,6 +151,20 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lon, dou //qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15); } +void QGCGoogleEarthView::clearTrail() +{ + // Check if the current trail has to be hidden + if (trailEnabled && !state) + { + QList mavs = UASManager::instance()->getUASList(); + foreach (UASInterface* currMav, mavs) + { + javaScript(QString("clearTrail(%1);").arg(currMav->getUASID())); + javaScript(QString("startTrail(%1);").arg(currMav->getUASID())); + } + } +} + void QGCGoogleEarthView::showTrail(bool state) { // Check if the current trail has to be hidden @@ -159,7 +173,7 @@ void QGCGoogleEarthView::showTrail(bool state) QList mavs = UASManager::instance()->getUASList(); foreach (UASInterface* currMav, mavs) { - javaScript(QString("hideTrail(%1);").arg(currMav->getUASID())); + javaScript(QString("clearTrail(%1);").arg(currMav->getUASID())); } } @@ -185,6 +199,7 @@ void QGCGoogleEarthView::follow(bool follow) { ui->followAirplaneCheckbox->setChecked(follow); followCamera = follow; + if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow)); } void QGCGoogleEarthView::goHome() @@ -343,6 +358,9 @@ void QGCGoogleEarthView::initializeGoogleEarth() ui->trailCheckbox->setChecked(trailEnabled); connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool))); + // Clear trail button + connect(ui->clearTrailButton, SIGNAL(clicked()), this, SLOT(clearTrail())); + // Go home connect(ui->goHomeButton, SIGNAL(clicked()), this, SLOT(goHome())); @@ -353,6 +371,8 @@ void QGCGoogleEarthView::initializeGoogleEarth() // Start update timer updateTimer->start(refreshRateMs); + follow(this->followCamera); + gEarthInitialized = true; } } @@ -395,11 +415,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.h b/src/ui/map3D/QGCGoogleEarthView.h index 80131dc7ed1079d3a23d133a797a2a4169c0450f..c9c84f079cb84b9fa1cbc5b818b0f943a879416e 100644 --- a/src/ui/map3D/QGCGoogleEarthView.h +++ b/src/ui/map3D/QGCGoogleEarthView.h @@ -82,6 +82,8 @@ public slots: void setActiveUAS(UASInterface* uas); /** @brief Update the global position */ void updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec); + /** @brief Clear the existing vehicle trails */ + void clearTrail(); /** @brief Show the vehicle trail */ void showTrail(bool state); /** @brief Show the waypoints */ diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui index 62ed1354b22f3e691b0e054ead26dbf001918605..56f0f4eb51c97041b3a4047d81fe7e173c5e7736 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 @@ -158,7 +158,7 @@ - + Delete all waypoints @@ -169,7 +169,7 @@ Delete all waypoints - Clear WPs + Clear Trails diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index 26ba1612905ad043d683df3e87105158ac21bb92..deb08f643c229d1d8312c649a10c78effa37b2c0 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -78,7 +78,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) removeChild(0, getNumChildren()); } - const QVector& list = uas->getWaypointManager().getWaypointList(); + const QVector& list = uas->getWaypointManager()->getWaypointList(); for (int i = 0; i < list.size(); i++) { diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 3a3f0d7d8ce5daf7aeff5cfabc92ab4061ab0f32..65f7f66a1e71b29af4e51b9a52f038b4a624fc17 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,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : localFrame(false), removeAction(new QAction("Delete this system", this)), renameAction(new QAction("Rename..", this)), + selectAction(new QAction("Control this system", this )), + selectAirframeAction(new QAction("Choose Airframe", this)), m_ui(new Ui::UASView) { m_ui->setupUi(this); @@ -102,6 +104,8 @@ 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(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); // Name changes @@ -129,21 +133,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 +330,6 @@ void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double if (!localFrame) { localFrame = true; - m_ui->gpsLabel->hide(); - m_ui->positionLabel->show(); } } @@ -419,11 +415,14 @@ void UASView::updateLoad(UASInterface* uas, double load) void UASView::contextMenuEvent (QContextMenuEvent* event) { QMenu menu(this); + menu.addAction(selectAction); + menu.addSeparator(); menu.addAction(renameAction); if (timeout) { menu.addAction(removeAction); } + menu.addAction(selectAirframeAction); menu.exec(event->globalPos()); } @@ -440,6 +439,34 @@ void UASView::rename() } } +void UASView::selectAirframe() +{ + if (uas) + { + // Get list of airframes from UAS + QStringList airframes; + airframes << "Generic" + << "Multiplex Easystar" + << "Multiplex Twinstar" + << "Multiplex Merlin" + << "Pixhawk Cheetah" + << "Mikrokopter" + << "Reaper" + << "Predator" + << "Coaxial" + << "Pteryx"; + + bool ok; + QString item = QInputDialog::getItem(this, tr("Select Airframe for %1").arg(uas->getUASName()), + tr("Airframe"), airframes, uas->getAirframe(), false, &ok); + if (ok && !item.isEmpty()) + { + // Set this airframe as UAS airframe + uas->setAirframe(airframes.indexOf(item)); + } + } +} + void UASView::refresh() { //setUpdatesEnabled(false); @@ -471,7 +498,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 +519,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..1654452da510a3ff1724c58881be1bf7b891f288 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -82,6 +82,8 @@ public slots: void setBackgroundColor(); /** @brief Bring up the dialog to rename the system */ void rename(); + /** @brief Select airframe for this vehicle */ + void selectAirframe(); protected: void changeEvent(QEvent *e); @@ -110,6 +112,8 @@ protected: bool localFrame; QAction* removeAction; QAction* renameAction; + QAction* selectAction; + QAction* selectAirframeAction; static const int updateInterval = 300;