From 5da9efd673c7fa9556fe871ec8c9f719b7d7ed5b Mon Sep 17 00:00:00 2001 From: PT_Dreamer Date: Wed, 6 Jun 2012 20:51:04 +0100 Subject: [PATCH] GCS/MapLib - Several fixes, changes and enhancements. --- .../src/libs/opmapcontrol/opmapcontrol.h | 19 + .../src/mapwidget/images/waypoint_marker1.png | Bin 0 -> 4359 bytes .../src/mapwidget/images/waypoint_marker2.png | Bin 0 -> 4385 bytes .../src/mapwidget/images/waypoint_marker3.png | Bin 0 -> 1613 bytes .../src/mapwidget/mapgraphicitem.cpp | 3 + .../src/mapwidget/mapresources.qrc | 3 + .../src/mapwidget/waypointcircle.cpp | 32 +- .../src/mapwidget/waypointcircle.h | 2 +- .../src/mapwidget/waypointitem.cpp | 16 +- .../opmapcontrol/src/mapwidget/waypointitem.h | 4 +- .../src/plugins/config/configgadgetwidget.h | 2 - .../openpilotgcs/src/plugins/opmap/opmap.pro | 2 +- .../opmap/opmap_edit_waypoint_dialog.cpp | 284 ++++-- .../opmap/opmap_edit_waypoint_dialog.h | 21 +- .../opmap/opmap_edit_waypoint_dialog.ui | 842 ++++++++++++------ .../plugins/opmap/opmap_overlay_widget.cpp | 2 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 88 +- .../src/plugins/opmap/opmapgadgetwidget.h | 18 +- .../src/plugins/opmap/pathplanmanager.cpp | 14 +- .../src/plugins/opmap/pathplanmanager.h | 15 - 20 files changed, 913 insertions(+), 454 deletions(-) create mode 100644 ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker1.png create mode 100644 ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker2.png create mode 100644 ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker3.png diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h index 4add132fd..aae8fd5ff 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.h @@ -1 +1,20 @@ +#ifndef OPMAP_CONTROL_H_ +#define OPMAP_CONTROL_H_ #include "src/mapwidget/opmapwidget.h" +namespace mapcontrol +{ + struct customData + { + float velocity; + int mode; + float mode_params[4]; + int condition; + float condition_params[4]; + int command; + int jumpdestination; + int errordestination; + }; + +} +Q_DECLARE_METATYPE(mapcontrol::customData) +#endif diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker1.png b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker1.png new file mode 100644 index 0000000000000000000000000000000000000000..179be20075e5c973c890e292034df9d521820aae GIT binary patch literal 4359 zcmV+i5%}(jP)KLZ*U+IBfRsybQWXdwQbLP>6pAqfylh#{fb6;Z(vMMVS~$e@S=j*ftg6;Uhf59&ghTmgWD0l;*T zI709Y^p6lP1rIRMx#05C~cW=H_Aw*bJ-5DT&Z2n+x)QHX^p z00esgV8|mQcmRZ%02D^@S3L16t`O%c004NIvOKvYIYoh62rY33S640`D9%Y2D-rV&neh&#Q1i z007~1e$oCcFS8neI|hJl{-P!B1ZZ9hpmq0)X0i`JwE&>$+E?>%_LC6RbVIkUx0b+_+BaR3cnT7Zv!AJxW zizFb)h!jyGOOZ85F;a?DAXP{m@;!0_IfqH8(HlgRxt7s3}k3K`kFu>>-2Q$QMFfPW!La{h336o>X zu_CMttHv6zR;&ZNiS=X8v3CR#fknUxHUxJ0uoBa_M6WNWeqIg~6QE69c9o#eyhGvpiOA@W-aonk<7r1(?fC{oI5N*U!4 zfg=2N-7=cNnjjOr{yriy6mMFgG#l znCF=fnQv8CDz++o6_Lscl}eQ+l^ZHARH>?_s@|##Rr6KLRFA1%Q+=*RRWnoLsR`7U zt5vFIcfW3@?wFpwUVxrVZ>QdQz32KIeJ}k~{cZZE^+ya? z2D1z#2HOnI7(B%_ac?{wFUQ;QQA1tBKtrWrm0_3Rgps+?Jfqb{jYbcQX~taRB;#$y zZN{S}1|}gUOHJxc?wV3fxuz+mJ4`!F$IZ;mqRrNsHJd##*D~ju=bP7?-?v~|cv>vB zsJ6IeNwVZxrdjT`yl#bBIa#GxRa#xMMy;K#CDyyGyQdMSxlWT#tDe?p!?5wT$+oGt z8L;Kp2HUQ-ZMJ=3XJQv;x5ci*?vuTfeY$;({XGW_huIFR9a(?@3)XSs8O^N5RyOM=TTmp(3=8^+zpz2r)C z^>JO{deZfso3oq3?Wo(Y?l$ge?uXo;%ru`Vo>?<<(8I_>;8Eq#KMS9gFl*neeosSB zfoHYnBQIkwkyowPu(zdms`p{<7e4kra-ZWq<2*OsGTvEV%s0Td$hXT+!*8Bnh2KMe zBmZRodjHV?r+_5^X9J0WL4jKW`}lf%A-|44I@@LTvf1rHjG(ze6+w@Jt%Bvjts!X0 z?2xS?_ve_-kiKB_KiJlZ$9G`c^=E@oNG)mWWaNo-3TIW8)$Hg0Ub-~8?KhvJ>$ z3*&nim@mj(aCxE5!t{lw7O5^0EIO7zOo&c6l<+|iDySBWCGrz@C5{St!X3hAA}`T4 z(TLbXTq+(;@<=L8dXnssyft|w#WSTW<++3>sgS%(4NTpeI-VAqb|7ssJvzNHgOZVu zaYCvgO_R1~>SyL=cFU|~g|hy|Zi}}s9+d~lYqOB71z9Z$wnC=pR9Yz4DhIM>Wmjgu z&56o6maCpC&F##y%G;1PobR9i?GnNg;gYtchD%p19a!eQtZF&3JaKv33gZ<8D~47E ztUS1iwkmDaPpj=$m#%)jCVEY4fnLGNg2A-`YwHVD3gv};>)hAvT~AmqS>Lr``i7kw zJ{5_It`yrBmlc25DBO7E8;5VoznR>Ww5hAaxn$2~(q`%A-YuS64wkBy=9dm`4cXeX z4c}I@?e+FW+b@^RDBHV(wnMq2zdX3SWv9u`%{xC-q*U}&`cyXV(%rRT*Z6MH?i+i& z_B8C(+grT%{XWUQ+f@NoP1R=AW&26{v-dx)iK^-Nmiuj8txj!m?Z*Ss1N{dh4z}01 z)YTo*JycSU)+_5r4#yw9{+;i4Ee$peRgIj+;v;ZGdF1K$3E%e~4LaI(jC-u%2h$&R z9cLXcYC@Xwnns&bn)_Q~Te?roKGD|d-g^8;+aC{{G(1^(O7m37Y1-+6)01cN&y1aw zoqc{T`P^XJqPBbIW6s}d4{z_f5Om?vMgNQEJG?v2T=KYd^0M3I6IZxbny)%vZR&LD zJpPl@Psh8QyPB@KTx+@RdcC!KX7}kEo;S|j^u2lU7XQ}Oo;f|;z4Ll+_r>@1-xl3| zawq-H%e&ckC+@AhPrP6BKT#_XdT7&;F71j}Joy zkC~6lh7E@6o;W@^IpRNZ{ptLtL(gQ-CY~4mqW;US7Zxvm_|@yz&e53Bp_lTPlfP|z zrTyx_>lv@x#=^!PzR7qqF<$gm`|ZJZ+;<)Cqu&ot2z=00004XF*Lt006O$eEU(80000WV@Og>004R=004l4008;_004mL004C` z008P>0026e000+nl3&F}000IeNkl7-k6~}+~zIh)rwx8|9&&E!mm_%un z29Xp|Dm4qBNZG)mfwDtLpdztDAb}8+k43v;SBXVn5d?xJ4XP+rR6rFxpP=d5}CA3=-JHX>No#$?*E)~)#1a3*|TR4b8~Z)%VmNfpxJD) zwzl@b>gwv}uU)(LiPhEBDtUOo0`NCr89+qX8rI&udzqh~XJllAD2fQf(9v3dzf>xH zYy0?kZEn}cIJm7!==Qu~BAw(g>F_;t-=wgwIQjBC7wXqSN{oLa`Ha9a3yr`6_0%!oG)c3}0 z`@sI4J46=>5C+g~LwgfC&0Jv+fH4%breYNs)f&gL`1YqCk(y&a1Uv(vMPwHE#z)4- zC>9EeFoaGEHXG1tLBF5>V3;5xXr(A>jcr-jN;5HHt1s-H2l$#&YTkgqob5F17Qe*ex70&aOCU)KmF&McP#O@!tK+#*=%;N-@JLN_#tEllm?~v%0s)~1$yeiU5E&2CL|^!Hijq};>IyR z-)b}(|K3UALs^GcUWP%P$ZmAV6qjbV@^w8D^99JA5)#kY*6cv5Qw!H;z|sO&+u{hBQq* z;P6YAm)F1gyFVzB7_`oTr7~!Z5g|$qZXB~2gfzmKUoTu#7h0Vb@T7=z?(}k{QUSOO zeDU{}uJ)h(>)$~a@|4RtaA-2TWJoKBXe0@Ly1dNUMl%8qi^#%l7ibY-baWKQaRA-| zzWL+RXLw`fI+RN}P-&9P(2HX>*(u-`cl%U_VTgz@IXTIW9XptrnW0*(K6AC# zcTZfpqOdH;46!k|X~r8j*SXqt0eCh_tIuzSv3I7vro<|c04tpy zE4jMo->=7Ynx=$di065T2-Rwpsi`R@CMN2YO6BE0U#lY`7_Io%+6I!Z(Z9Se5Fkks zJkKMFB1)wa6B83mPEO)D&WQ`{9^HYDWEtn0ZGe~WHH9n@!5Bjj1o*y>Qi{>hQ7p?s zDfL>X({bP4Y>jKBxH<3uPTmuEd;2Fzg75pJX-c_V#q$z`?QlPJ{{y_8b`(VzV-OLtEW;SHdZX8)+vx#Z`oDny!{LxH3`vrNLZLtq z1h}r-TD-Z2Ne#e_4+Pxm$uv#rcDpp2%|Vz9<<^4he<0v(`?#)45Ck+D4eIr}3k1Lj z(EdO`5yA63TCEl$LcLyReSO^p)&U!6-VgZS0RS0jkiDfM6OaG^002ovPDHLkV1mo@ BB#Qt5 literal 0 HcmV?d00001 diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker2.png b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/images/waypoint_marker2.png new file mode 100644 index 0000000000000000000000000000000000000000..7834aa5435a3297aff43f2df126b28a9040608c2 GIT binary patch literal 4385 zcmV++5#H{JP)KLZ*U+IBfRsybQWXdwQbLP>6pAqfylh#{fb6;Z(vMMVS~$e@S=j*ftg6;Uhf59&ghTmgWD0l;*T zI709Y^p6lP1rIRMx#05C~cW=H_Aw*bJ-5DT&Z2n+x)QHX^p z00esgV8|mQcmRZ%02D^@S3L16t`O%c004NIvOKvYIYoh62rY33S640`D9%Y2D-rV&neh&#Q1i z007~1e$oCcFS8neI|hJl{-P!B1ZZ9hpmq0)X0i`JwE&>$+E?>%_LC6RbVIkUx0b+_+BaR3cnT7Zv!AJxW zizFb)h!jyGOOZ85F;a?DAXP{m@;!0_IfqH8(HlgRxt7s3}k3K`kFu>>-2Q$QMFfPW!La{h336o>X zu_CMttHv6zR;&ZNiS=X8v3CR#fknUxHUxJ0uoBa_M6WNWeqIg~6QE69c9o#eyhGvpiOA@W-aonk<7r1(?fC{oI5N*U!4 zfg=2N-7=cNnjjOr{yriy6mMFgG#l znCF=fnQv8CDz++o6_Lscl}eQ+l^ZHARH>?_s@|##Rr6KLRFA1%Q+=*RRWnoLsR`7U zt5vFIcfW3@?wFpwUVxrVZ>QdQz32KIeJ}k~{cZZE^+ya? z2D1z#2HOnI7(B%_ac?{wFUQ;QQA1tBKtrWrm0_3Rgps+?Jfqb{jYbcQX~taRB;#$y zZN{S}1|}gUOHJxc?wV3fxuz+mJ4`!F$IZ;mqRrNsHJd##*D~ju=bP7?-?v~|cv>vB zsJ6IeNwVZxrdjT`yl#bBIa#GxRa#xMMy;K#CDyyGyQdMSxlWT#tDe?p!?5wT$+oGt z8L;Kp2HUQ-ZMJ=3XJQv;x5ci*?vuTfeY$;({XGW_huIFR9a(?@3)XSs8O^N5RyOM=TTmp(3=8^+zpz2r)C z^>JO{deZfso3oq3?Wo(Y?l$ge?uXo;%ru`Vo>?<<(8I_>;8Eq#KMS9gFl*neeosSB zfoHYnBQIkwkyowPu(zdms`p{<7e4kra-ZWq<2*OsGTvEV%s0Td$hXT+!*8Bnh2KMe zBmZRodjHV?r+_5^X9J0WL4jKW`}lf%A-|44I@@LTvf1rHjG(ze6+w@Jt%Bvjts!X0 z?2xS?_ve_-kiKB_KiJlZ$9G`c^=E@oNG)mWWaNo-3TIW8)$Hg0Ub-~8?KhvJ>$ z3*&nim@mj(aCxE5!t{lw7O5^0EIO7zOo&c6l<+|iDySBWCGrz@C5{St!X3hAA}`T4 z(TLbXTq+(;@<=L8dXnssyft|w#WSTW<++3>sgS%(4NTpeI-VAqb|7ssJvzNHgOZVu zaYCvgO_R1~>SyL=cFU|~g|hy|Zi}}s9+d~lYqOB71z9Z$wnC=pR9Yz4DhIM>Wmjgu z&56o6maCpC&F##y%G;1PobR9i?GnNg;gYtchD%p19a!eQtZF&3JaKv33gZ<8D~47E ztUS1iwkmDaPpj=$m#%)jCVEY4fnLGNg2A-`YwHVD3gv};>)hAvT~AmqS>Lr``i7kw zJ{5_It`yrBmlc25DBO7E8;5VoznR>Ww5hAaxn$2~(q`%A-YuS64wkBy=9dm`4cXeX z4c}I@?e+FW+b@^RDBHV(wnMq2zdX3SWv9u`%{xC-q*U}&`cyXV(%rRT*Z6MH?i+i& z_B8C(+grT%{XWUQ+f@NoP1R=AW&26{v-dx)iK^-Nmiuj8txj!m?Z*Ss1N{dh4z}01 z)YTo*JycSU)+_5r4#yw9{+;i4Ee$peRgIj+;v;ZGdF1K$3E%e~4LaI(jC-u%2h$&R z9cLXcYC@Xwnns&bn)_Q~Te?roKGD|d-g^8;+aC{{G(1^(O7m37Y1-+6)01cN&y1aw zoqc{T`P^XJqPBbIW6s}d4{z_f5Om?vMgNQEJG?v2T=KYd^0M3I6IZxbny)%vZR&LD zJpPl@Psh8QyPB@KTx+@RdcC!KX7}kEo;S|j^u2lU7XQ}Oo;f|;z4Ll+_r>@1-xl3| zawq-H%e&ckC+@AhPrP6BKT#_XdT7&;F71j}Joy zkC~6lh7E@6o;W@^IpRNZ{ptLtL(gQ-CY~4mqW;US7Zxvm_|@yz&e53Bp_lTPlfP|z zrTyx_>lv@x#=^!PzR7qqF<$gm`|ZJZ+;<)Cqu&ot2z=00004XF*Lt006O$eEU(80000WV@Og>004R=004l4008;_004mL004C` z008P>0026e000+nl3&F}000I&Nkl1JR`E+qF7f-;l|@($IrM&}j?in|N(cUzuCs(D51Z%Og`XI_`Pkb6V?*H-WF|QyAn3@V5u} zCkBz@53orWNCdQj7C08v+oVfvoH(Hqr_5EC`OfnboIF=+0uO1e&jH9HCK6dRFb-{H z95_754}Nv>W+Z+%+X7(!lY7W!1UVx^2I%iEh|vutsP1=&`*!(Wxd;nkY^QV@2Rablm`8HCvy#{*T5vMdz+@Diqg% zMiz`LPd&OD;BA+ZC`Aw^I9`k$#I!oDZnRwioXa*YE&auAuGWs-IW+d!Pw$2_Tmu$z zjBOrZ{}X%I|D^|d#{`uq{77MY5ti-K2@_6FH$)Js72t%l7FVvow4cCZ##escQcr3+7d<)Bb? zOA{-|8&J$cJ_k~uQ%$T=Tt8u@>(j72mMveuc=Up>oS@p9JV+HQk}z?AeJ@X3s(s;W zzZG}^WF(jcF!PX+s5B*r6kRu<+3~1%Lw^10lsG@zY5||oTDNcZa-qKnFatdDyJOR< z&;0Z#WHL}Rpj3dYBu-KsFQR4n)V+}3Ps}hm(+GflTI;DBDUdqV^lj|Ntdsyw17H6B z&rk5isjE=RLtg=eBuW$=C#2~{OikDIA5K*PzNxi-{nl8?!1gH)m<+#ng#O_&LW&oq zk@?Mce|Cb%jWoF|070baxB)H8QOYBg zr!UvK-NS#L7UZ*_6k(ju4O8BjTHx|x2jF|})IrkgxCFM3Q7{<3bA-`d<80VEe62WO z9(dvSMM5Xi#ELLf*j~WITk`;~0)Kl)VCV!mEt|-TFb7MF>=2A*qn1Lx$K zd7<+eBA~ipQC)NQ+`IL-Oriw8>*1`dqEk(Iq{7B+o2ZNpEfmVdBY%AJ8amaejNr|) zHT2pV{nNVw0U|%dY1ss}PtMFy85?A9dIy-gcS5`ER)R}2q|7~&#!dPTy73XDOhaU0I#hJ1kkA_@*~`~LlP$B`|@P-SyGj@ z{H}lYpI4Um4463*mDYPN2i83@KoZ7ymP_P^WOD|^vWZzKVOC0$SK1!uW|slZ_9pTA z4gpe?64*YW69SMka}@du6v~D3H7~(iZ1KO`{RdE|n#hX?Y#$XTXstzc>Z4l#4` zF=$W;$ym01h8t+o7BS2=3Fwg1#NThu@hu=t@e0;YRyt+HC1~Es5zc`ruO)>6y~0O9 z?g%igs;&}qUY`oAjZCaXE!g?hd|_6D)4PzMf!nt`-eBhKH45MSw8EL=0yG047e}TDu(NLuQu16uM#ttMYCrs| zWC>|FBYB^9ibn)yY1nhAGHOQaicWzHW(N?YXELzEg&1cx`54%j7;BF)`Q_m{1W^2} zRinnBZ0a(DA!W0o)%^|>2rj{T*LEeSEJ`dMOLZ5{$yCdgN-CAz=WVLL4G21p13@BL zu$)6iLbjH!W_!vVx_gsuuD?c8)Oy)-NC!Htduc7v-uByyf{K?|CKS%hSyYUgZZTpA zUeYBNZ)t2U$E-8Ye!OJHYxVEQov|2n@8r12=k14wLT>|2+ zP>Mt5txQ*G-yiQ?r0eCYF47ox1u7}oRwMuzQiSyq<_P6dc`6JVZsc$Vm%n8x`jm3g zg2{JAkLs7d_~onmrQWfybq z)q!skk?o&+E>Lamdf8`cg{4iFOf|!Hj9C#X-ZH6oXH4*{guv^0!{mgQwQAWzS2$j| zx@T11)AprI6D$D=aBPB!GinV}HkqUA-zRDGWUd4v40dT;V+DG98@SuUD*ivmvFY?1 zgLGU!BGUy!fJ0mOJ7L5qek$({9Y|yS`wVtsa$6}peMzhJP-woF{0$n|{G@oq4{cjd z!Z|K0DHkGLc)S#`XkdRr*oJeAPw)PJpDJdWDe@nWd#ZiHPt}jI>8GpPxbm3;@m7I; zM3K<+=)2+HbjoxWaqreUIY=?m7M>C0@hcRX=cAe>ehSXy`ac^rQ=4qg}8z za!AL0zi|9{M?h(tQJd_7BMn}&kXbJ95kH)Eu>*1^^rHe*M$71zOQiRr4z%lT9#^Ch&dSN zRzYJY&lIry(7mRRiiM6zWbktr6GQM8zIv4upTKq}*jky3`O~#%o!m8P%Om#$9JiHZ z$u@)pYDgIawAf`CyEA$rzLMGU#_lkUy*b<)P^N7-^6kK4V1V(Pg9PV7{gu61URjwB zogDcG0(7}1rtZjLi33`yH41Fm&48GBbU$|g1+1iv!eC&?5s1Q@?v3n~GR72^^*k`3 zOuNK@+(J&CN4#dsLNT*=;#7WY6QaR~0)%F;B3j5c6wFD=^_R2!yagIYy#8>2%mVg; zUP@X>SESAtF^modifiers() == Qt::ShiftModifier) { SetZoomToFitRect(SelectedArea()); + selectedArea=internals::RectLatLng::Empty; } } @@ -276,6 +277,8 @@ namespace mapcontrol { if(event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier)) this->setCursor(Qt::CrossCursor); + if(event->key()==Qt::Key_Escape) + selectedArea=internals::RectLatLng::Empty; QGraphicsItem::keyPressEvent(event); } void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc index b7a0b985c..b7709e2cc 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc @@ -12,6 +12,9 @@ images/mapquad.png images/dragons1.jpg images/dragons2.jpeg + images/waypoint_marker1.png + images/waypoint_marker2.png + images/waypoint_marker3.png images/airplanepip.png diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp index b61e073db..751d0788a 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp @@ -38,14 +38,17 @@ WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius,bool c connect(radius,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); connect(radius,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + refreshLocations(); + } -WayPointCircle::WayPointCircle(HomeItem *center, WayPointItem *radius, bool clockwise, MapGraphicItem *map, QColor color):my_center(center), +WayPointCircle::WayPointCircle(HomeItem *radius, WayPointItem *center, bool clockwise, MapGraphicItem *map, QColor color):my_center(center), my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) { - connect(center,SIGNAL(homePositionChanged(internals::PointLatLng)),this,SLOT(refreshLocations())); - connect(radius,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); - connect(radius,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + connect(radius,SIGNAL(homePositionChanged(internals::PointLatLng)),this,SLOT(refreshLocations())); + connect(center,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); + connect(center,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); + refreshLocations(); } int WayPointCircle::type() const @@ -53,21 +56,13 @@ int WayPointCircle::type() const // Enable the use of qgraphicsitem_cast with this item. return Type; } -QPainterPath WayPointCircle::shape() const -{ - QPainterPath path = QGraphicsEllipseItem::shape(); - path.addPolygon(arrowHead); - return path; -} + void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { QPointF p1; QPointF p2; - p1=my_center->pos(); - p2=my_center->pos(); - QLineF line(my_radius->pos(),my_center->pos()); - p1.ry()=p1.ry()+line.length(); - p2.ry()=p2.ry()-line.length(); + p1=QPointF(line.p1().x(),line.p1().y()+line.length()); + p2=QPointF(line.p1().x(),line.p1().y()-line.length()); QPen myPen = pen(); myPen.setColor(myColor); qreal arrowSize = 10; @@ -75,10 +70,8 @@ void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *op QBrush brush=painter->brush(); painter->setBrush(myColor); double angle =0; - if(myClockWise) + if(!myClockWise) angle+=Pi; - if (line.dy() >= 0) - angle = (Pi) - angle; QPointF arrowP1 = p1 + QPointF(sin(angle + Pi / 3) * arrowSize, cos(angle + Pi / 3) * arrowSize); @@ -104,8 +97,9 @@ void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *op void WayPointCircle::refreshLocations() { - QLineF line(my_center->pos(),my_radius->pos()); + line=QLineF(my_center->pos(),my_radius->pos()); this->setRect(my_center->pos().x(),my_center->pos().y(),2*line.length(),2*line.length()); + this->update(); } void WayPointCircle::waypointdeleted() diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h index 0f4ce7ce2..1c89fb798 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.h @@ -47,7 +47,6 @@ public: WayPointCircle(WayPointItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); WayPointCircle(HomeItem * center, WayPointItem * radius,bool clockwise,MapGraphicItem * map,QColor color=Qt::green); int type() const; - QPainterPath shape() const; void setColor(const QColor &color) { myColor = color; } private: @@ -57,6 +56,7 @@ private: QPolygonF arrowHead; QColor myColor; bool myClockWise; + QLineF line; protected: void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); public slots: diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index 9f6f87f6d..379d898a0 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -297,8 +297,10 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(fals emit WPValuesChanged(this); if(value) picture.load(QString::fromUtf8(":/markers/images/bigMarkerGreen.png")); - else + else if(this->flags() & QGraphicsItem::ItemIsMovable==QGraphicsItem::ItemIsMovable) picture.load(QString::fromUtf8(":/markers/images/marker.png")); + else + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); this->update(); } @@ -403,5 +405,17 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint):reached(fals setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); } + void WayPointItem::setFlag(QGraphicsItem::GraphicsItemFlag flag, bool enabled) + { + if(flag==QGraphicsItem::ItemIsMovable) + { + if(enabled) + picture.load(QString::fromUtf8(":/markers/images/marker.png")); + else + picture.load(QString::fromUtf8(":/markers/images/waypoint_marker2.png")); + } + QGraphicsItem::setFlag(flag,enabled); + } + int WayPointItem::snumber=0; } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h index e0bd594b5..aa9296f13 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h @@ -155,10 +155,9 @@ public: void RefreshPos(); void RefreshToolTip(); QPixmap picture; - QVariant customData(){return myCustomData;} - void setCustomData(QVariant arg){myCustomData=arg;} QString customString(){return myCustomString;} void setCustomString(QString arg){myCustomString=arg;} + void setFlag(GraphicsItemFlag flag, bool enabled); ~WayPointItem(); static int snumber; @@ -188,7 +187,6 @@ private: QTransform transf; HomeItem * myHome; wptype myType; - QVariant myCustomData; QString myCustomString; public slots: diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index e39cabfe8..3c7294eac 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -34,11 +34,9 @@ #include "objectpersistence.h" #include #include -//#include #include #include "utils/pathutils.h" #include -//#include "fancytabwidget.h" #include "utils/mytabbedstackwidget.h" #include "../uavobjectwidgetutils/configtaskwidget.h" diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap.pro b/ground/openpilotgcs/src/plugins/opmap/opmap.pro index 61b200a97..af12afa25 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap.pro +++ b/ground/openpilotgcs/src/plugins/opmap/opmap.pro @@ -1,6 +1,6 @@ +QT += webkit network TEMPLATE = lib TARGET = OPMapGadget - include(../../openpilotgcsplugin.pri) include(../../plugins/coreplugin/coreplugin.pri) include(../../libs/opmapcontrol/opmapcontrol.pri) diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index 2c7d20761..9d1761c5e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -27,8 +27,15 @@ #include "opmap_edit_waypoint_dialog.h" #include "ui_opmap_edit_waypoint_dialog.h" - +#include "opmapcontrol/opmapcontrol.h" // ********************************************************************* +typedef enum { MODE_FLYENDPOINT=0, MODE_FLYVECTOR=1, MODE_FLYCIRCLERIGHT=2, MODE_FLYCIRCLELEFT=3, + MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7, + MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; +typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2, + ENDCONDITION_LEGREMAINING=3, ENDCONDITION_ABOVEALTITUDE=4, ENDCONDITION_POINTINGTOWARDSNEXT=5, + ENDCONDITION_PYTHONSCRIPT=6, ENDCONDITION_IMMEDIATE=7 } EndConditionOptions; + // constructor opmap_edit_waypoint_dialog::opmap_edit_waypoint_dialog(QWidget *parent) : @@ -36,8 +43,33 @@ opmap_edit_waypoint_dialog::opmap_edit_waypoint_dialog(QWidget *parent) : ui(new Ui::opmap_edit_waypoint_dialog) { ui->setupUi(this); - waypoint_item = NULL; - connect(ui->rbRelative,SIGNAL(toggled(bool)),this,SLOT(setupWidgets(bool))); + my_waypoint = NULL; + connect(ui->rbRelative,SIGNAL(toggled(bool)),this,SLOT(setupPositionWidgets(bool))); + connect(ui->cbMode,SIGNAL(currentIndexChanged(int)),this,SLOT(setupModeWidgets())); + connect(ui->cbCondition,SIGNAL(currentIndexChanged(int)),this,SLOT(setupConditionWidgets())); + ui->cbMode->addItem("Fly Direct",MODE_FLYENDPOINT); + ui->cbMode->addItem("Fly Vector",MODE_FLYVECTOR); + ui->cbMode->addItem("Fly Circle Right",MODE_FLYCIRCLERIGHT); + ui->cbMode->addItem("Fly Circle Left",MODE_FLYCIRCLELEFT); + + ui->cbMode->addItem("Drive Direct",MODE_DRIVEENDPOINT); + ui->cbMode->addItem("Drive Vector",MODE_DRIVEVECTOR); + ui->cbMode->addItem("Drive Circle Right",MODE_DRIVECIRCLELEFT); + ui->cbMode->addItem("Drive Circle Left",MODE_DRIVECIRCLERIGHT); + + ui->cbMode->addItem("Fixed Attitude",MODE_FIXEDATTITUDE); + ui->cbMode->addItem("Set Accessory",MODE_SETACCESSORY); + ui->cbMode->addItem("Disarm Alarm",MODE_DISARMALARM); + + ui->cbCondition->addItem("None",ENDCONDITION_NONE); + ui->cbCondition->addItem("Timeout",ENDCONDITION_TIMEOUT); + ui->cbCondition->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET); + ui->cbCondition->addItem("Leg remaining",ENDCONDITION_LEGREMAINING); + ui->cbCondition->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE); + ui->cbCondition->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT); + ui->cbCondition->addItem("Python script",ENDCONDITION_PYTHONSCRIPT); + ui->cbCondition->addItem("Immediate",ENDCONDITION_IMMEDIATE); + } // destrutor @@ -65,7 +97,7 @@ void opmap_edit_waypoint_dialog::on_pushButtonOK_clicked() int res = saveSettings(); if (res < 0) return; - waypoint_item = NULL; + my_waypoint = NULL; close(); } @@ -77,17 +109,10 @@ void opmap_edit_waypoint_dialog::on_pushButtonApply_clicked() void opmap_edit_waypoint_dialog::on_pushButtonRevert_clicked() { - ui->checkBoxLocked->setChecked(original_locked); - ui->spinBoxNumber->setValue(original_number); - ui->doubleSpinBoxLatitude->setValue(original_coord.Lat()); - ui->doubleSpinBoxLongitude->setValue(original_coord.Lng()); - ui->doubleSpinBoxAltitude->setValue(original_altitude); - ui->lineEditDescription->setText(original_description); - - saveSettings(); + loadFromWP(my_waypoint); } -void opmap_edit_waypoint_dialog::setupWidgets(bool isRelative) +void opmap_edit_waypoint_dialog::setupPositionWidgets(bool isRelative) { ui->lbLong->setVisible(!isRelative); ui->lbDegLong->setVisible(!isRelative); @@ -103,85 +128,216 @@ void opmap_edit_waypoint_dialog::setupWidgets(bool isRelative) ui->doubleSpinBoxBearing->setVisible(isRelative); } +void opmap_edit_waypoint_dialog::setupModeWidgets() +{ + ModeOptions mode=(ModeOptions)ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); + switch(mode) + { + case MODE_FLYENDPOINT: + case MODE_FLYVECTOR: + case MODE_FLYCIRCLERIGHT: + case MODE_FLYCIRCLELEFT: + case MODE_DRIVEENDPOINT: + case MODE_DRIVEVECTOR: + case MODE_DRIVECIRCLELEFT: + case MODE_DRIVECIRCLERIGHT: + case MODE_DISARMALARM: + ui->modeParam1->setVisible(false); + ui->modeParam2->setVisible(false); + ui->modeParam3->setVisible(false); + ui->modeParam4->setVisible(false); + ui->dsb_modeParam1->setVisible(false); + ui->dsb_modeParam2->setVisible(false); + ui->dsb_modeParam3->setVisible(false); + ui->dsb_modeParam4->setVisible(false); + break; + case MODE_FIXEDATTITUDE: + ui->modeParam1->setText("pitch"); + ui->modeParam2->setText("roll"); + ui->modeParam3->setText("yaw"); + ui->modeParam4->setText("throttle"); + ui->modeParam1->setVisible(true); + ui->modeParam2->setVisible(true); + ui->modeParam3->setVisible(true); + ui->modeParam4->setVisible(true); + ui->dsb_modeParam1->setVisible(true); + ui->dsb_modeParam2->setVisible(true); + ui->dsb_modeParam3->setVisible(true); + ui->dsb_modeParam4->setVisible(true); + break; + case MODE_SETACCESSORY: + ui->modeParam1->setText("Acc.channel"); + ui->modeParam2->setText("Value"); + ui->modeParam1->setVisible(true); + ui->modeParam2->setVisible(true); + ui->modeParam3->setVisible(false); + ui->modeParam4->setVisible(false); + ui->dsb_modeParam1->setVisible(true); + ui->dsb_modeParam2->setVisible(true); + ui->dsb_modeParam3->setVisible(false); + ui->dsb_modeParam4->setVisible(false); + break; + } +} +void opmap_edit_waypoint_dialog::setupConditionWidgets() +{ + EndConditionOptions mode=(EndConditionOptions)ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); + switch(mode) + { + case ENDCONDITION_NONE: + case ENDCONDITION_IMMEDIATE: + case ENDCONDITION_PYTHONSCRIPT: + ui->condParam1->setVisible(false); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(false); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + break; + case ENDCONDITION_TIMEOUT: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Timeout(ms)"); + break; + case ENDCONDITION_DISTANCETOTARGET: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(true); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(true); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Distance(m)"); + ui->condParam2->setText("Flag(0=2D,1=3D)");//FIXME + break; + case ENDCONDITION_LEGREMAINING: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Relative Distance(0=complete,1=just starting)"); + break; + case ENDCONDITION_ABOVEALTITUDE: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Altitude in meters (negative)"); + break; + case ENDCONDITION_POINTINGTOWARDSNEXT: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Degrees variation allowed"); + break; + default: + + break; + } +} void opmap_edit_waypoint_dialog::on_pushButtonCancel_clicked() { - waypoint_item = NULL; + my_waypoint = NULL; close(); } -// ********************************************************************* - int opmap_edit_waypoint_dialog::saveSettings() { - // ******************** - // fetch the various ui item values - - bool locked = ui->checkBoxLocked->isChecked(); - int number = ui->spinBoxNumber->value(); if (number < 0) { return -1; } + customData data; + data.mode=ui->cbMode->itemData(ui->cbMode->currentIndex()).toInt(); + data.mode_params[0]=ui->dsb_modeParam1->value(); + data.mode_params[1]=ui->dsb_modeParam2->value(); + data.mode_params[2]=ui->dsb_modeParam3->value(); + data.mode_params[3]=ui->dsb_modeParam4->value(); - double latitude = ui->doubleSpinBoxLatitude->value(); - double longitude = ui->doubleSpinBoxLongitude->value(); + data.condition=ui->cbCondition->itemData(ui->cbCondition->currentIndex()).toInt(); + data.condition_params[0]=ui->dsb_condParam1->value(); + data.condition_params[1]=ui->dsb_condParam2->value(); + data.condition_params[2]=ui->dsb_condParam3->value(); + data.condition_params[3]=ui->dsb_condParam4->value(); - double altitude = ui->doubleSpinBoxAltitude->value(); + QVariant var; + var.setValue(data); + my_waypoint->setData(0,var); - QString description = ui->lineEditDescription->displayText().simplified(); - - // ******************** - // transfer the settings to the actual waypoint - - waypoint_item->SetNumber(number); - waypoint_item->SetCoord(internals::PointLatLng(latitude, longitude)); - waypoint_item->SetAltitude(altitude); - waypoint_item->SetDescription(description); - waypoint_item->setFlag(QGraphicsItem::ItemIsMovable, !locked); + my_waypoint->SetNumber(ui->spinBoxNumber->value()); + my_waypoint->SetCoord(internals::PointLatLng(ui->doubleSpinBoxLatitude->value(), ui->doubleSpinBoxLongitude->value())); + my_waypoint->SetAltitude(ui->doubleSpinBoxAltitude->value()); + my_waypoint->SetDescription(ui->lineEditDescription->displayText().simplified()); + my_waypoint->setFlag(QGraphicsItem::ItemIsMovable, !ui->checkBoxLocked->isChecked()); if(ui->rbAbsolute->isChecked()) - waypoint_item->setWPType(mapcontrol::WayPointItem::absolute); + my_waypoint->setWPType(mapcontrol::WayPointItem::absolute); else - waypoint_item->setWPType(mapcontrol::WayPointItem::relative); + my_waypoint->setWPType(mapcontrol::WayPointItem::relative); mapcontrol::distBearing pt; pt.distance=ui->spinBoxDistance->value(); pt.bearing=ui->doubleSpinBoxBearing->value()/180*M_PI; - this->waypoint_item->setRelativeCoord(pt); - // ******************** - + my_waypoint->setRelativeCoord(pt); return 0; // all ok } -// ********************************************************************* -// public functions +void opmap_edit_waypoint_dialog::loadFromWP(mapcontrol::WayPointItem *waypoint_item) +{ + customData data=waypoint_item->data(0).value(); + ui->spinBoxNumber->setValue(waypoint_item->Number()); + ui->doubleSpinBoxLatitude->setValue(waypoint_item->Coord().Lat()); + ui->doubleSpinBoxLongitude->setValue(waypoint_item->Coord().Lng()); + ui->doubleSpinBoxAltitude->setValue(waypoint_item->Altitude()); + ui->lineEditDescription->setText(waypoint_item->Description()); + if(waypoint_item->WPType()==mapcontrol::WayPointItem::absolute) + ui->rbAbsolute->setChecked(true); + else + ui->rbRelative->setChecked(true); + ui->doubleSpinBoxBearing->setValue(waypoint_item->getRelativeCoord().bearing*180/M_PI); + ui->spinBoxDistance->setValue(waypoint_item->getRelativeCoord().distance); + + ui->cbMode->setCurrentIndex(ui->cbMode->findData(data.mode)); + ui->dsb_modeParam1->setValue(data.mode_params[0]); + ui->dsb_modeParam2->setValue(data.mode_params[1]); + ui->dsb_modeParam3->setValue(data.mode_params[2]); + ui->dsb_modeParam4->setValue(data.mode_params[3]); + + ui->cbCondition->setCurrentIndex(ui->cbCondition->findData(data.condition)); + ui->dsb_condParam1->setValue(data.condition_params[0]); + ui->dsb_condParam2->setValue(data.condition_params[1]); + ui->dsb_condParam3->setValue(data.condition_params[2]); + ui->dsb_condParam4->setValue(data.condition_params[3]); +} void opmap_edit_waypoint_dialog::editWaypoint(mapcontrol::WayPointItem *waypoint_item) { if (!waypoint_item) return; - this->waypoint_item = waypoint_item; - - original_number = this->waypoint_item->Number(); - original_locked = (this->waypoint_item->flags() & QGraphicsItem::ItemIsMovable) == 0; - original_coord = this->waypoint_item->Coord(); - original_altitude = this->waypoint_item->Altitude(); - original_description = this->waypoint_item->Description().simplified(); - original_type=this->waypoint_item->WPType(); - original_distance=this->waypoint_item->getRelativeCoord().distance; - original_bearing=this->waypoint_item->getRelativeCoord().bearing*180/M_PI; - ui->checkBoxLocked->setChecked(original_locked); - ui->spinBoxNumber->setValue(original_number); - ui->doubleSpinBoxLatitude->setValue(original_coord.Lat()); - ui->doubleSpinBoxLongitude->setValue(original_coord.Lng()); - ui->doubleSpinBoxAltitude->setValue(original_altitude); - ui->lineEditDescription->setText(original_description); - if(original_type==mapcontrol::WayPointItem::absolute) - ui->rbAbsolute->setChecked(true); - else - ui->rbRelative->setChecked(true); - ui->doubleSpinBoxBearing->setValue(original_bearing); - ui->spinBoxDistance->setValue(original_distance); - setupWidgets(ui->rbRelative->isChecked()); + this->my_waypoint = waypoint_item; + loadFromWP(waypoint_item); + setupPositionWidgets(ui->rbRelative->isChecked()); show(); } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h index b6fde4828..0270afa3d 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.h @@ -35,6 +35,7 @@ namespace Ui { class opmap_edit_waypoint_dialog; } +using namespace mapcontrol; class opmap_edit_waypoint_dialog : public QDialog { @@ -50,31 +51,21 @@ public: */ void editWaypoint(mapcontrol::WayPointItem *waypoint_item); + void loadFromWP(mapcontrol::WayPointItem *waypoint_item); protected: void changeEvent(QEvent *e); private: Ui::opmap_edit_waypoint_dialog *ui; - - int original_number; - bool original_locked; - internals::PointLatLng original_coord; - double original_altitude; - QString original_description; - double original_distance; - double original_bearing; - - - mapcontrol::WayPointItem::wptype original_type; - - mapcontrol::WayPointItem *waypoint_item; - + mapcontrol::WayPointItem * my_waypoint; int saveSettings(); private slots: private slots: - void setupWidgets(bool isRelative); + void setupModeWidgets(); + void setupPositionWidgets(bool isRelative); + void setupConditionWidgets(); void on_pushButtonCancel_clicked(); void on_pushButtonRevert_clicked(); void on_pushButtonApply_clicked(); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.ui b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.ui index 84de38a2d..f77fcc965 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.ui +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.ui @@ -9,8 +9,8 @@ 0 0 - 546 - 261 + 571 + 375 @@ -31,258 +31,604 @@ - - - - - - 0 - 0 - - - - Number - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 2 + + + + Position + + + + + 0 + 10 + 528 + 266 + + + + + + + 0 + 0 + + + + Latitude + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + true + + + + 0 + 0 + + + + Longitude + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Altitude + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + meters + + + + + + + + 0 + 0 + + + + Description + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + 200 + + + + + + + + 0 + 0 + + + + 7 + + + -90.000000000000000 + + + 90.000000000000000 + + + + + + + 7 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + -5000.000000000000000 + + + 5000.000000000000000 + + + + + + + degrees + + + + + + + degrees + + + + + + + + 0 + 0 + + + + Type + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Relative + + + + + + + Absolute + + + + + + + 999999999 + + + + + + + meters + + + + + + + degrees + + + + + + + Bearing + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 360.000000000000000 + + + + + + + + 0 + 0 + + + + Distance + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Qt::LeftToRight + + + Locked + + + + + + + Number + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + - - - - - - 0 - 0 - - - - Latitude - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + Mode + + + + + -1 + -1 + 511 + 281 + + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Qt::LeftToRight + + + Mode + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + 999999999.000000000000000 + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param1 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param2 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param3 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param4 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 999999999.000000000000000 + + + + + + + 999999999.000000000000000 + + + + + + + 999999999.000000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + - - - - - true - - - - 0 - 0 - - - - Longitude - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + End condition + + + + + 0 + 0 + 551 + 291 + + + + QLayout::SetDefaultConstraint + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Qt::LeftToRight + + + Condition + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + 999999999.000000000000000 + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param1 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param2 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param3 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + Qt::LeftToRight + + + param4 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 999999999.000000000000000 + + + + + + + 999999999.000000000000000 + + + + + + + 999999999.000000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + - - - - - - 0 - 0 - - - - Altitude - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - meters - - - - - - - - 0 - 0 - - - - Description - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - Qt::RightToLeft - - - Locked - - - - - - - 200 - - - - - - - - 0 - 0 - - - - 7 - - - -90.000000000000000 - - - 90.000000000000000 - - - - - - - 7 - - - -180.000000000000000 - - - 180.000000000000000 - - - - - - - -5000.000000000000000 - - - 5000.000000000000000 - - - - - - - degrees - - - - - - - degrees - - - - - - - - 0 - 0 - - - - Type - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Relative - - - - - - - Absolute - - - - - - - 999999999 - - - - - - - meters - - - - - - - degrees - - - - - - - Bearing - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - 360.000000000000000 - - - - - - - - 0 - 0 - - - - Distance - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + + + + Page + + + diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_overlay_widget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_overlay_widget.cpp index 4c7f7aeab..ac056f36b 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_overlay_widget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_overlay_widget.cpp @@ -32,7 +32,7 @@ opmap_overlay_widget::opmap_overlay_widget(QWidget *parent) : QWidget(parent), ui(new Ui::opmap_overlay_widget) { - ui->setupUi(this); + } opmap_overlay_widget::~opmap_overlay_widget() diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 3dabcd91e..4fd28242c 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -206,14 +206,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals connect(m_map, SIGNAL(OnTileLoadStart()), this, SLOT(OnTileLoadStart())); // tile loading start signals - connect(m_map, SIGNAL(OnMapDrag()), this, SLOT(OnMapDrag())); // map drag signals - connect(m_map, SIGNAL(OnMapZoomChanged()), this, SLOT(OnMapZoomChanged())); // map zoom changed - connect(m_map, SIGNAL(OnMapTypeChanged(MapType::Types)), this, SLOT(OnMapTypeChanged(MapType::Types))); // map type changed - connect(m_map, SIGNAL(OnEmptyTileError(int, core::Point)), this, SLOT(OnEmptyTileError(int, core::Point))); // tile error connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals - // connect(m_map, SIGNAL(WPNumberChanged(int const&,int const&,WayPointItem*)), this, SLOT(WPNumberChanged(int const&,int const&,WayPointItem*))); - // connect(m_map, SIGNAL(WPInserted(int const&, WayPointItem*)), this, SLOT(WPInserted(int const&, WayPointItem*))); - // connect(m_map, SIGNAL(WPDeleted(int,WayPointItem*)), this, SLOT(WPDeleted(int,WayPointItem*))); connect(m_map,SIGNAL(OnWayPointDoubleClicked(WayPointItem*)),this,SLOT(wpDoubleClickEvent(WayPointItem*))); m_map->SetCurrentPosition(m_home_position.coord); // set the map position m_map->Home->SetCoord(m_home_position.coord); // set the HOME position @@ -490,7 +483,7 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addSeparator()->setText(tr("Waypoints")); contextMenu.addAction(wayPointEditorAct); - contextMenu.addAction(addWayPointAct); + contextMenu.addAction(addWayPointActFromContextMenu); if (m_mouse_waypoint) { // we have a waypoint under the mouse @@ -701,10 +694,6 @@ void OPMapGadgetWidget::zoomChanged(double zoomt, double zoom, double zoomd) zoomAct.at(index0_zoom)->setChecked(true); // set the right-click context menu zoom level } -void OPMapGadgetWidget::OnMapDrag() -{ -} - void OPMapGadgetWidget::OnCurrentPositionChanged(internals::PointLatLng point) { if (!m_widget || !m_map) @@ -754,47 +743,6 @@ void OPMapGadgetWidget::OnTileLoadComplete() m_widget->progressBarMap->setVisible(false); } -void OPMapGadgetWidget::OnMapZoomChanged() -{ -} - -void OPMapGadgetWidget::OnMapTypeChanged(MapType::Types type) -{ - Q_UNUSED(type); -} - -void OPMapGadgetWidget::OnEmptyTileError(int zoom, core::Point pos) -{ - Q_UNUSED(zoom); - Q_UNUSED(pos); -} - -void OPMapGadgetWidget::WPNumberChanged(int const &oldnumber, int const &newnumber, WayPointItem *waypoint) -{ - Q_UNUSED(oldnumber); - Q_UNUSED(newnumber); - Q_UNUSED(waypoint); -} - -/** - TODO: slot to do something upon Waypoint insertion - */ -void OPMapGadgetWidget::WPInserted(int const &number, WayPointItem *waypoint) -{ - Q_UNUSED(number); - Q_UNUSED(waypoint); -} - -/** - TODO: slot to do something upon Waypoint deletion - */ -void OPMapGadgetWidget::WPDeleted(int const &number, WayPointItem *waypoint) -{ - Q_UNUSED(number); - Q_UNUSED(waypoint); -} - - void OPMapGadgetWidget::on_toolButtonZoomP_clicked() { QMutexLocker locker(&m_map_mutex); @@ -1334,11 +1282,16 @@ void OPMapGadgetWidget::createActions() wayPointEditorAct->setEnabled(true); // temporary connect(wayPointEditorAct, SIGNAL(triggered()), this, SLOT(onOpenWayPointEditorAct_triggered())); - addWayPointAct = new QAction(tr("&Add waypoint"), this); - addWayPointAct->setShortcut(tr("Ctrl+A")); - addWayPointAct->setStatusTip(tr("Add waypoint")); - connect(addWayPointAct, SIGNAL(triggered()), this, SLOT(onAddWayPointAct_triggered())); - this->addAction(addWayPointAct); + addWayPointActFromContextMenu = new QAction(tr("&Add waypoint"), this); + addWayPointActFromContextMenu->setShortcut(tr("Ctrl+A")); + addWayPointActFromContextMenu->setStatusTip(tr("Add waypoint")); + connect(addWayPointActFromContextMenu, SIGNAL(triggered()), this, SLOT(onAddWayPointAct_triggeredFromContextMenu())); + + addWayPointActFromThis = new QAction(tr("&Add waypoint"), this); + addWayPointActFromThis->setShortcut(tr("Ctrl+A")); + addWayPointActFromThis->setStatusTip(tr("Add waypoint")); + connect(addWayPointActFromThis, SIGNAL(triggered()), this, SLOT(onAddWayPointAct_triggeredFromThis())); + this->addAction(addWayPointActFromThis); editWayPointAct = new QAction(tr("&Edit waypoint"), this); editWayPointAct->setShortcut(tr("Ctrl+E")); @@ -1738,8 +1691,16 @@ void OPMapGadgetWidget::onOpenWayPointEditorAct_triggered() { waypoint_editor_dialog.show(); } +void OPMapGadgetWidget::onAddWayPointAct_triggeredFromContextMenu() +{ + onAddWayPointAct_triggered(m_context_menu_lat_lon); +} +void OPMapGadgetWidget::onAddWayPointAct_triggeredFromThis() +{ + onAddWayPointAct_triggered(lastLatLngMouse); +} -void OPMapGadgetWidget::onAddWayPointAct_triggered() +void OPMapGadgetWidget::onAddWayPointAct_triggered(internals::PointLatLng coord) { if (!m_widget || !m_map) return; @@ -1747,20 +1708,11 @@ void OPMapGadgetWidget::onAddWayPointAct_triggered() if (m_map_mode != Normal_MapMode) return; - // m_waypoint_list_mutex.lock(); - - // create a waypoint on the map at the last known mouse position - internals::PointLatLng coord; - if(this->contextMenu.isVisible()) - coord = m_context_menu_lat_lon; - else - coord=lastLatLngMouse; m_map->WPCreate(coord, 0, ""); //wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); //wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index 50441b837..a2df40978 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -153,19 +153,8 @@ private slots: void OnCurrentPositionChanged(internals::PointLatLng point); void OnTileLoadComplete(); void OnTileLoadStart(); - void OnMapDrag(); - void OnMapZoomChanged(); - void OnMapTypeChanged(MapType::Types type); - void OnEmptyTileError(int zoom, core::Point pos); void OnTilesStillToLoad(int number); - /** - * Unused for now, hooks for future waypoint support - */ - void WPNumberChanged(int const& oldnumber,int const& newnumber, WayPointItem* waypoint); - void WPInserted(int const& number, WayPointItem* waypoint); - void WPDeleted(int const& number, WayPointItem* waypoint); - /** * @brief mouse right click context menu signals */ @@ -190,7 +179,9 @@ private slots: void onFollowUAVheadingAct_toggled(bool checked); void onOpenWayPointEditorAct_triggered(); - void onAddWayPointAct_triggered(); + void onAddWayPointAct_triggeredFromContextMenu(); + void onAddWayPointAct_triggeredFromThis(); + void onAddWayPointAct_triggered(internals::PointLatLng coord); void onEditWayPointAct_triggered(); void onLockWayPointAct_triggered(); void onDeleteWayPointAct_triggered(); @@ -280,7 +271,8 @@ private: QAction *followUAVheadingAct; QAction *wayPointEditorAct; - QAction *addWayPointAct; + QAction *addWayPointActFromThis; + QAction *addWayPointActFromContextMenu; QAction *editWayPointAct; QAction *lockWayPointAct; QAction *deleteWayPointAct; diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp index c4ac4f8e1..00a1734a3 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp @@ -64,21 +64,25 @@ void pathPlanManager::on_WPInserted(int wp_number, WayPointItem * wp) data.mode=PathAction::MODE_FLYENDPOINT; data.condition=PathAction::ENDCONDITION_NONE; data.velocity=0; - wp->customData().setValue(data); + QVariant var; + var.setValue(data); + wp->setData(0,var); refreshOverlays(); } void pathPlanManager::on_WPValuesChanged(WayPointItem * wp) { } - +//typedef enum { MODE_FLYENDPOINT=0, MODE_FLYVECTOR=1, MODE_FLYCIRCLERIGHT=2, +//MODE_FLYCIRCLELEFT=3, MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, +//MODE_DRIVECIRCLERIGHT=7, MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; void pathPlanManager::refreshOverlays() { QMutexLocker locker(&wplistmutex); myMap->deleteAllOverlays(); foreach(WayPointItem * wp,*waypoints) { - customData data=wp->customData().value(); + customData data=wp->data(0).value(); switch(data.mode) { case PathAction::MODE_FLYENDPOINT: @@ -92,10 +96,14 @@ void pathPlanManager::refreshOverlays() break; case PathAction::MODE_FLYCIRCLERIGHT: case PathAction::MODE_DRIVECIRCLERIGHT: + if(wp->Number()==0) + myMap->WPCircleCreate((HomeItem*)myMap->Home,wp,true); myMap->WPCircleCreate(findWayPointNumber(wp->Number()-1),wp,true); break; case PathAction::MODE_FLYCIRCLELEFT: case PathAction::MODE_DRIVECIRCLELEFT: + if(wp->Number()==0) + myMap->WPCircleCreate((HomeItem*)myMap->Home,wp,false); myMap->WPCircleCreate(findWayPointNumber(wp->Number()-1),wp,false); break; default: diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h index 0868d44eb..c17e61441 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h @@ -34,22 +34,7 @@ #include "waypoint.h" #include "QMutexLocker" #include "QPointer" -namespace mapcontrol -{ - struct customData - { - float velocity; - int mode; - float mode_params[4]; - int condition; - float condition_params[4]; - int command; - int jumpdestination; - int errordestination; - }; -} -Q_DECLARE_METATYPE(mapcontrol::customData) namespace Ui { class pathPlanManager; }